Paper Title: Vision-based Wearable Steering Assistance for People with Impaired Vision in Jogging.



Significant Contributions of the Higgs Mode and Self-Energy Corrections to Low-Frequency Complex Conductivity in DC-Biased Superconducting Devices
    Takayuki Kubo
    August 5, 2024
==================================================================================================================================================

empty
empty



§ ABSTRACT 
	
Outdoor sports pose a challenge for people with impaired vision. 
The demand for higher-speed mobility inspired us to develop a vision-based wearable steering assistance. 
To ensure broad applicability, we focused on a representative sports environment, the athletics track.
Our efforts centered on improving the speed and accuracy of perception, enhancing planning adaptability for the real world, and providing swift and safe assistance for people with impaired vision. In perception, we engineered a lightweight multitask network capable of simultaneously detecting track lines and obstacles. 

Additionally, due to the limitations of existing datasets for supporting multi-task detection in athletics tracks, we diligently collected and annotated a new dataset (MAT) containing 1000 images.
In planning, we integrated the methods of sampling and spline curves, addressing the planning challenges of curves. Meanwhile, we utilized the positions of the track lines and obstacles as constraints to guide people with impaired vision safely along the current track.
Our system is deployed on an embedded device, Jetson Orin NX. Through outdoor experiments, it demonstrated adaptability in different sports scenarios, assisting users in achieving free movement of 400-meter at an average speed of 1.34 m/s, meeting the level of normal people in jogging.
Our MAT dataset is publicly available from

<https://github.com/snoopy-l/MAT>








§ INTRODUCTION


According to statistics from the World Health Organization (WHO), nearly 253 million people worldwide are facing visual impairments <cit.>, which affects both their physical health and quality of life. 
Sports, like jogging, becomes particularly challenging for people with impaired vision, as they lack the visual cues essential for safe navigation. Even in Paralympic athletics, visually impaired athletes rely on guide runners, rendering independent running unattainable.

Advancements in artificial intelligence and wearable devices led to the development of assisting the blind and visually impaired (BVI) in overcoming common challenges in their daily lives<cit.>. For instance, <cit.> provides navigation speeds of 0.21 m/s, <cit.> facilitated walking speeds of 0.6 m/s, and <cit.> enables people with impaired vision to walk through hallways at speeds of 1 m/s.
These studies have contributed to enhancing the mobility of them. 
Nonetheless, there remains a dearth of exploration in sports and exercise, leaving the quest for faster independent mobility largely uncharted. Facilitating assistance for people with impaired vision in sports can also be beneficial for their physical health. Consequently, the design of wearable steering assistance tailored for people with impaired vision in sports holds crucial practical significance.

 

Based on visual sensors, we have designed a wearable steering assistance to guide people with impaired vision in independent movement within a representative sports scenario, the athletics track. The hardware of the assistance is illustrated in  Fig. <ref> and the overview is shown in Fig. <ref>. 
The perception and navigation capabilities developed within the athletics track hold the potential for broad applicability in sports, fitness, and competitions.
However, they also come with challenges.
Detecting the track lines presents difficulties because they are typically thin and can be easily obstructed by people ahead. Furthermore, there is an implicit correlation between the track lines and people obstacles. Additionally, the uniqueness of the scenario makes existing datasets cannot be directly used. The difference between curve and straight also poses a challenge for planning. 

To tackle these issues, we meticulously annotated an athletics track dataset for multitasks.
Meanwhile, we proposed a lightweight and efficient multitask network, enabling continuous track segmentation even in partial occlusion and the detection of people obstacles, bridging the gap between the two tasks. 
Moreover, we employ a combination of sampling and spline curves to effectively address the problem of curves. Our planner leverages the positions of track lines and obstacles as constraints to mitigate computational complexity. 
Even been deployed on embedded devices, our system operates efficiently, delivering swift and accurate directional instructions. Our main contributions are:


	
  * We designed a lightweight and efficient multitask network to achieve efficient and accurate obstacle detection and track line segmentation in embedded device. Additionally, we curated and annotated a dataset tailored for multitask in athletics track.
	
  * We developed a wearable steering assistance for people with impaired vision in jogging, enabling users achieving free movement of 400-meter at an average speed of 1.34 m/s, meeting the level of normal people.

 


§ RELATED WORKS

Based on visual sensors, many studies have developed devices suitable for people with impaired vision.

<cit.> uses visual and tactile feedback to assist the BVI in tasks of navigating mazes, locating chairs, and moving through crowds.
<cit.> use guidance robot to provide hospital navigation for people with impaired Vision.
<cit.> uses a visual positioning system to assist in indoor navigation for the BVI.
<cit.> employs an RGB-D camera to detect blind lanes and obstacles, using an improved artificial potential field method for navigation.
<cit.> can meet the needs of navigation, target localization, facial recognition, and text reading, attaining speeds of 0.21 m/s during navigation tasks.


<cit.> adds a camera to the white cane for object positioning, and combines multiple sensors to assist people with impaired vision in improving their walking speed of 0.6 m/s.
Relevant researches have greatly facilitated the lives of the BVI and improved their mobility.
But they cannot meet the demand for sports.
Assisting people with impaired vision in achieving higher-speed mobility within the athletics track needs to accomplish two essential tasks: environmental perception and path planning.

Detection and segmentation methods<cit.><cit.> can be employed for the perception of obstacles captured by RGB-D camera.
In terms of track line detection, we draw inspiration from lane detection in autonomous driving.


Deep learning-based lane detection methods include detection-based methods like LaneATT <cit.> and Ultra-Fast-Lane-Detection<cit.>. Segmentation-based methods include SCNN <cit.> and RESA<cit.>. While segmentation-based methods may struggle with occlusion issues, the detected lane lines are accurate for positioning. YOLOP<cit.> proposed using a single network to simultaneously perform tasks like traffic object detection, drivable area segmentation, and lane detection. YOLOPv2<cit.> further improves the performance and speed of each task. In terms of training data, there are many publicly available datasets in autonomous driving, such as BDD100K <cit.>, Cityscapes <cit.>. However, there is no dataset specifically designed for athletics track.

In terms of path planning, purely relying on a single method has its limitations, many research began to integrate different planning methods to achieve better results.
Baidu Apollo's EM planner <cit.> employs optimization function combined with polynomial curve for trajectory planning, utilizing dynamic programming and quadratic programming for path and speed planning respectively.<cit.> combines RRT and Dijkstra in path planning, where RRT is employed to reduce the planning area, while Dijkstra's optimizer is used to obtain the final output path. <cit.> combines graph-based search methods with sampling-based methods to quickly obtain the optimal solution.




§ METHODOLOGY





 §.§ System Overview
 
 
The overview of the assistance is shown in Fig. <ref>. 
The user is equipped with an RGB-D camera, which continuously captures real-time color and depth images. 
These inputs are processed within the perception module, which employs a multitask network with two decoder heads to perform track lines segmentation and obstacles detection, effectively establishing connections between the track and people.
In path planning module, we employ spline curves to facilitate the transformation between Frenet and Cartesian coordinates, addressing curve challenges.
Within Frenet coordinates, we use the track boundaries and obstacle positions as constraints for average sampling, reducing the sampling area and lowering the computational burden. We employ dynamic programming to calculate the minimum path cost between sampling points, yielding a locally optimal trajectory.
Finally, the path planning results are transformed into directional commands, guiding the user through voice feedback.



 §.§ Hardware Setup

 
As shown in Fig. <ref>, the wearable steering assistance comprises a pair of RGB-D glasses, an embedded device, and audio feedback. 
The wearable glasses are equipped with a RealSense D435i depth camera encased in a 3D-printed housing, enabling real-time capture of both RGB and depth images.
The embedded device is Jetson Orin NX, equipped with a processing capability of 100 TOPS.
This device is responsible for processing camera inputs and converting them into voice output.
The system is powered by a lithium battery and can operate continuously for 5 hours, meeting the endurance requirements for outdoor activities. 
The glasses themselves weigh 125g, while the embedded device weighs 274g and the overall weight is 1 kg, making it convenient to carry. 
The software is deployed on the Jetson Orin NX, eliminating the need for a remote server. The whole set of equipment fulfills the portability requirements.





 §.§ Dataset Construction




Given the uniqueness of the athletics track, we undertook the task of collecting, annotating, and curating an athletics track dataset, termed as Multi-data for Athletics Track (MAT), specifically tailored for multi-task detection. Our dataset comprises 1k track images, divided into training, evaluation, and test subsets at an 8:1:1 ratio. Each of these images was annotated with detection bounding boxes and track line masks. 
Some examples are shown in Fig. <ref>.
 

Our dataset features 2.47k instances of person. In terms of track line annotations, we have 376 images in straight and 624 images in curve. We performed continuous annotations on the track that were partial occlusion, resulting in 319 images with occlusions and 681 images without occlusion.











 §.§ Multi-task Network




We aim to achieve precise environment perception while operating on embedded devices. 
However, given the constraints of computational cost and memory, employing separate models for each task is impractical. 

To address this, we turn to multi-task networks, which leverage a shared backbone to extract valuable features and optimize performance across different tasks with separate decoders.
Therefore, we adopted an efficient and lightweight multi-task network capable of simultaneously handling track line segmentation and people detection tasks.

YOLOP <cit.> is capable of simultaneously performing tasks like traffic object detection, drivable area segmentation, and lane detection. However, it cannot be directly applied in the athletics track due to varying task requirements and speed constraints. To alleviate the challenge, we made a series of improvements to it.

Firstly, our tasks are different. We focus on people detection and track line segmentation for planning. Therefore, in addition to the data labeling efforts mentioned in section <ref>, we pruned down the detection head used for drivable area segmentation. 
The modified network now includes a shared encoder and two separate decoders for different tasks.
Secondly, we needed to meet the speed requirements for detection on embedded devices. Hence, we replaced the backbone with a lightweight architecture suitable for embedded devices, GhostNet <cit.>. Additionally, we replaced the spatial pyramid pooling module with spatial pyramid pooling-fast to reduce the computational load while maintaining performance. 
The above design may lead to a decrease in feature extraction capability. 
Consequently, we introduced the Coordinate Attention(CA) <cit.> at the end of the backbone to balance speed and accuracy without increasing computational demands.

In the detection head, we utilize FPN to propagate semantic features top-down and PAN for localization feature propagation bottom-up, achieving effective fusion.
In the track line segmentation head, we harnessed deep information from FPN. Through three upsampling processes, we acquire high-precision outputs, ensuring the continuity of track line predictions even in cases of partial occlusion.

we name our network RunnerNet, which substantially reduces the number of parameters and floating point operations compared to the original network. The network structure is shown in Fig. <ref>.



 §.§ Path Planning and Navigation







Given the track's unique characteristics, we employ an approach to combine averaged sampling with spline curves, which effectively leverages track information.


This strategy employs spline curves to convert track lines from Cartesian coordinates to Frenet coordinates, facilitating path planning within the Frenet coordinates. 

We confine the sampling region within the track lines, limiting the planning space and reducing computational complexity. 
Uniform sampling is then performed within this track region, and sample points are selected by loss function to yield locally optimal trajectory points.
Finally, the trajectory is transformed back to Cartesian coordinates. The yaw angle of the path is converted into voice output, providing guidance to users. 



  §.§.§ Utilizing cubic spline for the transformation between Frenet and Cartesian
 

Representing curves in Cartesian coordinates is challenging. By transforming the curved boundaries from the Cartesian coordinates to the Frenet coordinates, nonlinear constraints can be converted into linear constraints. However, directly utilizing formulas for conversion is complex.  
We employ cubic spline curves S_i(x) = a_i + b_ix + c_ix^2 + d_ix^3 for implicit solving. Unknown parameters a_i, b_i,  and d_i can be represented by c_i.

Performing cubic spline separately on the x and y coordinates, the curve can be uniquely represented as:

    S(t) = ( x(t),y(t)),

where S(t) represents the length of the curve in the Frenet coordinates, x(t) and y(t) is the cubic spline curves constructed under varying parameters. 
The arc length S and yaw angle ψ of the curve can be derived as:

    S = √(x'^2 + y'^2),


    ψ  = arctan (y'/x').

where x', y' is the derivative of x, y.
Given the length S_i, along with the segment interval i and interval parameters a_i, b_i, c_i, and d_i, it is possible to reverse the transformation from Frenet coordinates to Cartesian coordinates to obtain the  x and y coordinates. 



  §.§.§ Averaging sampling and cost function
 
EM planner<cit.> defines a reference line to connect global planning with local planning.
What sets us apart is that the detected track lines provide natural constraints for sampling. Utilizing spline curves, we represent the two boundaries of the track in the Frenet coordinates and incorporate the positions of obstacles. With knowledge of the track width and forward viewing distance, we uniformly sample n × m points both laterally and longitudinally.
While traditional average sampling requires large computation, the constraints of tracks significantly reduce the computational workload. 
Unlike road environments, average sampling and calculating cost function within the narrow space of the track can provide more comprehensive information.

After constructing the sampling space, each point could be evaluated by the summation of the cost function. The cost C_total between sample points can be expressed as:

    C_total = C_dis + C_obs,

where C_dis represents the distance cost between the current sampling point s_1 and the next sampling point s_2, and C_dis can be defined as:

    C_dis = . s_1 - s_2..

When there are obstacles, beyond the safe distance d_safe, the collision cost C_obs is inversely proportional to the collision distance d_col, the scale factor is k, C_obs can be defined as:

    C_obs = {[              0    no obstacle;        k/d_col d_col > d_safe;              ∞ d_col < d_safe ].


















We define the cost of the sampling points in the last row as the distance from the middle point.
Using the dynamic programming, we iterate the cost between sampling points from the last row forward. We set the initial point as the origin of the camera coordinates and iterate from front to back, selecting the sampling points with the minimum cost for each row as the trajectory points.
Finally, we use the cubic spline curve to fit the trajectory points to form a smooth path and obtain the yaw angle of the planned path by coordinate transformation.



  §.§.§ Audio Feedback
 
Effective feedback to the BVI is the key to achieving human-computer interaction and ensuring the availability of the system. We divided the visual field in front of the BVI into five sections corresponding to five direction instructions: forward, left forward, right forward, turn left, and turn right. The yaw angle of the planned output will fall into the corresponding interval, and the planning result will be converted into the direction instruction through voice feedback. In order to ensure real-time feedback, the trajectory planning is open-loop, and one direction instruction will be output for each frame.






































































§ EXPERIMENTS

In this section, we introduce the dataset setting, parameter setting, and experimental content. Experiments in section <ref> and section <ref> were carried out based on the configuration environment of NVIDIA GeForce RTX3060.
We ran all tests under the same experimental settings and evaluation metrics.
In section <ref>, we conducted outdoor experiments on the embedded device, Jetson Orin NX. 



 §.§ Settings

The BDD100K dataset <cit.> is used for multi-task detection in autonomous driving. Although the algorithms trained on the BDD100K dataset have strong transferability,
as shown in Fig. <ref>, due to the uniqueness of the athletics track, the model trained on BDD100K cannot be directly used for track.



We compared the detection performance of models trained on BDD100K, on our dataset MAT, and further fine-tuned on MAT after BDD100K's pre-traind. The results are presented in Table <ref>.
Utilizing BDD100K as a pre-trained model led to improvements in track line segmentation and yielded people detection results similar to models trained on our dataset. Consequently, we evaluate network performance by using models pre-trained on BDD100K and fine-tuned on MAT.

We resize images in BDD100K from 1280×720 to 640×384, and the images in MAT is 640×480.
In pre-training, we employ an Adam optimizer for 240 epochs with a 3-epoch warm-up, a batch size of 24, an initial learning rate of 0.001, and a weight decay of 0.0005. For fine-tuning, we train the models for an additional 240 epochs with a batch size of 24, and a learning rate of 0.001.








 §.§ Performance of Multitask Networks

The visualization of the network detection result is illustrated in  Fig. <ref>.
In section <ref>, we made improvements by pruning detection heads (-Head), changing the backbone (+GhostNet), and introducing attention mechanisms (RunnerNet). 
The impact of parameters and the performance of networks are shown in Table <ref>. 
We use Acc to evaluate the segmentation performance and use Recall and mAP_50 as the evaluation metric for detection accuracy.


Since the lack of prior works for multitask detection in the athletics track, we compared the performance of modified networks (-one head, +GhostNet, and RunnerNet).
The results indicate that our network reduced the FLOPs of the original network by 44% and improved the speed by 35%.
Although the modified model is smaller than the original network, it has achieved higher detection accuracy. 
RunnerNet achieved 92.6% accuracy in track line segmentation and achieved better detection performance compared to model (-Head). 
But its performance in detection is slightly lower than the model (+GhostNet), possibly due to its focus on the runway line.
To ensure the accuracy of track segmentation for the planning module, we choose RunnerNet, which performs best in track line segmentation, as the final model.

















































 §.§ Outdoor Experiments


To test the real-world usability of the system, three healthy subjects (User1, female, User2, male, and User3, male) were invited to wear the assistance for experiments, all participants wore blindfolds for the same level of vision.

In path planning, the sampling distance in the forward direction is 10 meters in Cartesian coordinates, with a sampling interval of 1 meter in Frenet coordinates and 5 sampling points in the horizontal direction. 

We categorized the sports scenarios into three situations:

	
  * Safe: no obstacle present, or obstacles have no impact.
	
  * Detour: detour around the obstacle and return.
	
  * Switch: change different track to bypass obstacles.


As shown in Fig. <ref>, we conducted tests to evaluate the users' performance in different sports scenarios.


Users were able to complete tasks of moving forward, detouring, and changing tracks with assistance.


In addition to testing their performance in various sports scenarios, subjects also wore the assistance for free movement experiments, including 100-meter straight, 100-meter curve, and  400-meter track. The number of experiments and average speeds are shown in Table <ref>.

The results indicate that, with the assistance of the device, users can achieve an average movement speed of 1.52 m/s in straight, 1.23 m/s in curve, and 1.34 m/s in track, reaching the level of normal people in jogging. Due to the processing speed limitations of embedded devices, we may not reach the speed of quick running, but it is sufficient to provide people with impaired vision with opportunities to participate in sports.




§ CONCLUSIONS


Based on visual sensors, we have developed a wearable steering assistance designed for people with impaired vision in jogging. 

We conducted outdoor experiments on the athletics track, and the results show that our wearable assistance enables the people with impaired vision to achieve an average speed of 1.34 m/s in track. The increase in speed is attributed to the accuracy of perception and the operational speed.
While our device provides faster mobility, it may not meet the speed requirements of quick running. In the future, we will continue to enhance the sports experience for people with impaired vision.

IEEEtran



-12cm   
                                  
                                  
                                  
                                  
                                  





















































 Paper Title: SmileyNet -- Towards the Prediction of the Lottery by Reading Tea Leaves with AI.




.jpg,.pdf,.mps,.png

The Bulk Densities of Small Solar System Bodies as a Probe of Planetesimal Formation
    [
    August 5, 2024
====================================================================================



§ ABSTRACT 
We introduce SmileyNet, a novel neural network with psychic abilities. It is inspired by the fact that a positive mood can lead to improved cognitive capabilities including classification tasks. The network is hence presented in a first phase with smileys and an encouraging loss function is defined to bias it into a good mood. SmileyNet is then used to forecast the flipping of a coin based on an established method of Tasseology, namely by reading tea leaves. Training and testing in this second phase are done with a high-fidelity simulation based on real-world pixels sampled from a professional tea-reading cup. SmileyNet has an amazing accuracy of 72% to correctly predict the flip of a coin. Resnet-34, respectively YOLOv5 achieve only 49%, respectively 53%. It is then shown how multiple SmileyNets can be combined to win the lottery.     






§ INTRODUCTION

Modern AI is one of the greatest if not the greatest scientific breakthrough of all times. The publication of the structure of DNA by Watson and Crick in Nature <cit.> has only achieved slightly more than 19,000 citations by summer 2024 according to GoogleScholar. And it has taken 71 years. AI <cit.> has led to many important papers, which received many more citations within a much shorter time-frame; see, e.g., Tab. <ref> for a few examples based on citations according to GoogleScholar.



Given the relevance of AI for our modern world, it is surprising that there are areas of high economic relevance where it has not been applied, yet. We explore here - up to our knowledge for the very first time - the potential of AI for the psychic services industry, which is a multi-billion dollar business. This industry is estimated to have reached a revenue of well above $2 billion in the USA in 2023. And it experiences a steady further growth. This holds especially for psychic online services that managed an exponential growth in recent years according to market-reports.       

We suspect that AI may already be used in some real-world psychic services, especially in the online branches. But the methods and the results are not academically published up to our knowledge - likely due to the large economic potential they possess. While this is somewhat understandable, we believe in Open Science <cit.> and we share our insights due to the large benefits Psychic AI can provide to humanity.

A first medium-term goal is to win the lottery to further fund research in the novel field of Psychic AI. A well-established method of Tasseology (also known as Tasseomancy or Tasseography) <cit.> is used to this end, namely reading tea leaves <cit.>. In the spirit of divide and conquer, the simpler problem of predicting the outcome of flipping a coin is tackled first. We then show how this can be used to predict the numbers for winning the lottery jackpot. 

As an important basic research result, it is proven in this paper, up to our knowledge for the very first time, that a neural network can possess psychic powers. Concretely, it is shown that the novel SmileyNet can predict the flipping of a coin with a precision well above chance. 




     


§ SMILEYNET


SmileyNet is a novel bio-inspired network. It is motivated by the fact that cognitive processes can be positively influenced by a positive mood 
<cit.>.
This includes especially also classification tasks <cit.>. 



Furthermore, it is well-known that visual stimuli are well suited to elicit emotions <cit.>. This includes in particular a smiling face <cit.>. We hence first present the network with smileys as input to bias it into a good mood. More precisely, the Unicode emoticon "U+1F642" for the classical smiley, officially known as "Slightly Smiling Face" is used to this end (Fig.<ref>).

But stress is a negative factor for cognition <cit.>. We do not want to loose the positive effects of the good mood by putting the network under stress in the smiley training phase. Hence, it gets positive feedback <cit.> that it is doing fine, i.e., that it is doing nothing wrong.

More precisely, the training in the smileyfication phase is done as follows. A quadratic loss function L_smile() is used. To avoid putting the network under stress and to give a further positive endorsement, the expected output ŷ is set to be the actual output y whenever a smiley is presented to the net, i.e., ŷ = f_I(y) where f_I() is the identity function. So,


    L_smile()= 1/2 (y - ŷ)^2  with ŷ = f_I(y)


This method of smileyfication can be applied to any network N_old to turn it into a novel one N_new, which is expected to perform better than the previous N_old. 

Often, there may only be small improvements and it can happen that multiple trials are needed until some improvement is seen for the first time, which should then be immediately documented. Also, it can be useful to take additional factors into account. For example, the effects of smileyfication may only be seen on a sunny day as, e.g., the weather has an influence on the mood <cit.>.  



§ RELATED WORK


YOLOv5 <cit.> and Resnet34 <cit.> are well-known networks for image classification. They are used for comparison to SmileyNet in Sec. <ref>. There are also many other networks. But as YOLOv5 and Resnet32 are very popular, we concentrate here on those two. 





YOLOv5 is the fifth iteration of the revolutionary "You Only Look Once" object detection model. It is designed to deliver high-speed, high-accuracy results in real-time. Built on PyTorch, this powerful deep learning framework has garnered immense popularity for its versatility, ease of use, and high performance. Fig. <ref> shows how it works.


ResNet-34 is a convolutional neural network in PyTorch. It is has 34 layers. It addresses the problem of vanishing gradient. This is done with the identity shortcut connection that skips one or more layers. 
Fig. <ref> shows how it works.




§ HIGH-FIDELITY SIMULATION


The constellation of tea leaves and the predicted event are linked by cosmic energy. We use this fact to generate high-fidelity training data. This is not easy in the context of psychic powers and it may be one reason why AI has not been used - up to our knowledge - in this economically highly relevant business up to now.

The core idea of our approach for training data is to use the reverse direction of the link. The coin is flipped in a deterministic way, i.e., it is so to say turned by hand in rounds r from one side to the other. The related constellation of tea-leaves, i.e., the image I_r is then generated afterwards. 

In the software implementation, "heads" is presented with a "1" and "tails" with a "0" for the sake of convenience and wlog. Also wlog, we start with "tails" facing up as initialization of the flipping process. In the very first round r=1, the coin is turned and it hence shows "heads". The coin is then turned again and it now shows "tails" in round r=2, and so on. More formally, it holds for the outcome o_flip(r) in round r that


    o_flip(r) = 
    heads    if  (r+1)  mod  2 = 0
    tails    otherwise



The images I_r in our high-fidelity simulation are based on a real-world image (Fig. <ref>)[wikimedia.org; curid=17659832; user: MochaSwirl; Public Domain] of tea-leaves in a cup used by a fortune-telling professional. From this professional image, real-world pixels p_tea belonging to tea and real-world pixels p_cup belonging to the cup are extracted. This forms the base image I_0 with 100 × 100 pixels (Fig. <ref>). For the sake of simplicity, p_tea-pixels are shown in black and white is used for p_cup-pixels . 




The first image I_1 is then generated right after the coin was flipped for the first time. So, I_1 is linked to producing "head" according to the laws of Tasseology. In general, each image I_r is generated from I_r-1 by randomly selecting k_change coordinates [x,y]_j : x,y ∈{0,...,99} that are altered as follows 


    I_r[x][y] = 
    
        p_tea   if  I_r-1[x][y] = p_cup
    
        p_cup   if  I_r-1[x][y] = p_tea











Each I_r is generated right after doing the flip of the coin in round r, i.e., it is linked to the outcome o_flip(r) of flipping the coin in that round. According to our experience, small to medium values for k_change work best. The process is illustrated in Fig. <ref>. As mentioned, the p_cup, respectively p_tea pixel are shown as white, respectively black pixel in Fig. <ref> for the sake of simplicity. 



§ EXPERIMENTS AND RESULTS

  
The high-fidelity simulation presented in Sec. <ref> is used to generate 500 images. When it is needed by the network, the images are scaled to fit the requirements of PyTorch. The set is split into the first 300 images that are used for training and the following 200 images are used for validation. 


 


Tab. <ref> shows the results for YOLOv5 and Resnet-34. Resnet-34 has only 98 out of 200 cases correctly predicted. YOLOv5 does a bit better and it manages 106 correct predictions on the testing set.

Due to the better performance of YOLOv5 compared to Resnet-34, it is used for a smileyfication to generate SmileyNet version 1. As SmileyNet is expected to have an improved performance, it is trained with less data. Only every fifth image from the data-set is used. Note that this is still an alternating sequence of "heads" and "tails", i.e., it is exactly the same principle as before, but just 100 training images. The remaining 400 images are used for testing.

SmileyNet manages an amazing accuracy of 71.75%, i.e., 287 out of 400 cases are correctly predicted. This is well above the performance of YOLOv5 and Resnet-34 (Tab. <ref>). Most importantly, it is well above chance and we therefore prove - up to our knowledge - for the very first time that AI can possess psychic powers. Note that it can be shown that the improvement is statistically significant, i.e., it is a thorough and sound result.       

Tab. <ref> summarizes the performance gains compared to YOLOv5 and Resnet-34. SmileyNet has an accuracy of 71.75%, i.e., there is an increase by 35.38% from the 53% of YOLOv5. The performance gain is in the comparison with Resnet-34 with 46.43% even higher. SmileyNet requires only 100 training images while YOLOv5 and Resnet-34 had 300 images. So, there is a reduction by 200 images or 66% in comparison to the other two nets. 



§ SMILEYNET FOR LOTTERY FORECASTS



The forecasting of throwing a coin can already be a useful psychic power. But the forecast of lotteries has a very straightforward economical potential. Lotteries exist in most countries around the globe with high jackpots for the correctly predicted numbers. It is shown in the following with the example of the German lottery how SmileyNet can be used to this end.

In the German lottery, six numbers win_1,...,win_6 are drawn out of a pool with the numbers 1,...,49. The psychic power of SmileyNet can forecast the flip of a coin, i.e., 1 bit. We therefore simply combine multiple SmileyNets to encode the numbers 1,...,49. Then, one after the other win_i is predicted. 

More precisely, let b_j denote the forecasting output of SmileyNet N_j and ⟨ b_1,...,b_n ⟩_2 denotes the number x encoded by the bits b_1,...,b_n, i.e.,

    x = ⟨ b_1,...,b_n ⟩_2 = ∑_i=1^nb_i · 2^n-i


The i-th number win_i of a lottery draw is then predicted with ten SmileyNets N_j with outputs b_j as follows 

    win_i = ⟨ b_1,...,b_5 ⟩_2 + ⟨ b_6,...,b_9 ⟩_2 + ⟨ b_10⟩_2


As mentioned, the winning numbers win_1,...,win_6 are simply generated one after the other. It must of course be ensured that fresh images of tea-leaves from a just emptied cup are provided in every round. Nonetheless, SmileyNet does not have perfect psychic power. It may hence happen in rare occasions that a number win_j+k has been predicted before as win_j. But this is not a problem. Then, the current win_j+k is simply discarded and an other round is added to determine a new win_j+k.

Given the predictive power of SmileyNet to correctly forecast a bit with p_ N=0.7175, the exact likelihood p_win of forecasting the German lottery with the above method can be computed as

    p_win   = p_ N^5 ·p_ N^4 · p_ N
       = 0.03615922 
       = 3.615922 · 10^-2
        
This likelihood may look small at first glance, but it is by six orders of magnitude better than just chance with

    p_chance   = 49  6^-1
       = (49!/6!(49-6)!)^-1
       = 1/13,983,816
       = 7.15112 · 10^-8
       ≪ p_win
        

This is an important result that demonstrates the huge potential of the novel field of Psychic AI. Note that it is a thorough result as it can be shown that it is statistically significant.

But before we apply these results in practice, we still want to improve the performance of our approach. This holds first and foremost for the likelihood p_win of predicting the correct numbers for winning the jackpot:

	
  * One option is simply more training data and more computation power for a larger network to turn SmileyNet into a foundation model for psychic AI. 

  * Furthermore, we expect a transformer to be more accurate given that the drawing of lottery numbers is a weekly event in Germany. This is left for future work.

Also, a reduction of the required number of SmileyNets would be nice before applying it in practice as it is a bit inconvenient to drink so much tea. 



§ CONCLUSIONS


SmileyNet version 1 was introduced. It is based on smileyfication, i.e., the fact that a positive mood can have a positive influence on cognitive functions including classification. It was shown how SmileyNet can be used to predict the lottery by reading tea-leaves. First, the flipping of a coin was predicted with a very good accuracy, namely 72%. State of the art networks like YOLOv5 and Resnet-34 perform much worse. Second, it was shown how multiple SmileyNets can be combined for lottery forecasts. In addition to the potential practical implications, it is a major breakthrough to show that AI can have psychic powers.  



§ ACKNOWLEDGMENTS

We thank our anonymous reviewers for their feedback. Special thanks go to reviewer 2, who pointed out that the drawing of lottery numbers tends to be a weekly or at least regular event, and that the use of a transformer should be beneficial. 




§ EPILOG

This is a satirical accumulation of misconceptions, mistakes, and flawed reasoning I have encountered in recent times as a reviewer and sometimes even as a reader of published papers. I hope it is entertaining and useful in the context of the education of BSc, MSc, and PhD students in Machine Learning, Artificial Intelligence, and Cognitive Science. 

For an easy start, here a few questions to reflect upon. First and foremost, there is of course the following puzzle: 

	
  * Where does the "magic" hide that is presented here? How can SmileyNet "predict" the flip of a coin better than chance? 

As a small hint: there is a single conceptual mistake that explains it. 

There are also many other bugs, respectively misconceptions and flawed reasoning in this paper. Here some further food for thought, to hopefully teach a good lesson by setting a bad example:   

	
  * Can AI beat natural laws? What are the pros and cons of a detailed pre-analysis of the possible information content of the data?
	
  * How much academic novelty is in SmileyNet? How much does it differ from the net it is build upon? 
	
  * How do we prevent smileyfication in our own work? How can we properly show that an "extension" or "modification" of an existing approach leads to a substantial improvement and study the related trade-offs?  
	
  * What is a proper assessment of the state of the art? How do we find, understand, and discuss related work?
	
  * What is a proper statistical performance analysis? What is statistical significance and how should it be interpreted?
	
  * When and how do I expect my own work to be of relevance? Why do I publish it? 
	
  * What are Artificial Intelligence and Machine Learning? Which sub-areas within these disciplines exist? What are commonly used methodologies within the sub-areas? 
		
  * What is the role of training data in supervised machine learning? What are the pros and cons of simulation?
	
  * What is Open Science? What are its principles? What does it imply for the quality of the underlying work? What are the pros and cons if others, respectively oneself adheres to it? 
	
  * What is a bio-inspired design? How can insights from Cognitive Science be used and which pitfalls exist?
	
  * What are the pros and cons of bibliometrics? How can they help us to identify relevant work? And where are their constraints and limitations?

This list is a bit concentrated on the "big questions". It is of course also possible to just concentrate on the (many) methodological mistakes in the SmileyNet paper. So, the above list is neither meant to be exhaustive nor to be without dispensable aspects depending on the context. It is just a possible starting point for using the SmileyNet in the class-room, or to just meditate about it on your own, or... 

Enjoy and have fun...

10

url@samestyle








WatsonCrick-DNA-Nature1953
J. D. Watson and F. H. C. Crick, “Molecular structure of nucleic acids: A
  structure for deoxyribose nucleic acid,” Nature, vol. 171, no. 4356,
  pp. 737–738, 1953. [Online]. Available:
  <https://doi.org/10.1038/171737a0>


DeepLearning-Hinton-Nature15
Y. LeCun, Y. Bengio, and G. Hinton, “Deep learning,” Nature, vol. 521,
  no. 7553, pp. 436–444, 2015. [Online]. Available:
  <https://doi.org/10.1038/nature14539>


DeepLearning-Book-MITpress16
I. Goodfellow, Y. Bengio, and A. Courville, Deep learning.1em
  plus 0.5em minus 0.4emMIT press, 2016.

DeepLearning-ResNet-CVPR16
K. He, X. Zhang, S. Ren, and J. Sun, “Deep residual learning for image
  recognition,” in IEEE Conference on Computer Vision and Pattern
  Recognition (CVPR), 2016, Conference Proceedings, pp. 770–778.

Adam-StochasticOptimization-arXiv2014
J. B. Diederik P Kingma, “Adam: A method for stochastic optimization,”
  arXiv preprint, vol. arXiv:1412.6980, 2014.

DeepLearning-AlexNet-NIPS12
A. Krizhevsky, I. Sutskever, and G. E. Hinton, “Imagenet classification with
  deep convolutional neural networks,” pp. 1106–1114, 2012. [Online].
  Available:
  <http://papers.nips.cc/paper/4824-imagenet-classification-with-deep-convolutional-neural-networks>


Breimann-RandomForests-ML01
L. Breiman, “Random forests,” Machine Learning, vol. 45, no. 1, pp.
  5–32, 2001. [Online]. Available:
  <https://doi.org/10.1023/A:1010933404324>


DeepCNN-LargeScale-ImageRecognition-2014
K. Simonyan and A. Zisserman, “Very deep convolutional networks for
  large-scale image recognition,” arXiv preprint, vol. arXiv:1409.1556,
  2014.

GAN-NIPS2014
I. Goodfellow, J. Pouget-Abadie, M. Mirza, B. Xu, D. Warde-Farley, S. Ozair,
  A. Courville, and Y. Bengio, “Generative adversarial nets,” 2014. [Online].
  Available:
  <https://proceedings.neurips.cc/paper_files/paper/2014/file/5ca3e9b122f61f8f06494c97b1afccf3-Paper.pdf>


FasterR-CNN-NIPS2015
S. Ren, K. He, R. Girshick, and J. Sun, “Faster r-cnn: Towards real-time
  object detection with region proposal networks,” 2015. [Online]. Available:
  <https://proceedings.neurips.cc/paper_files/paper/2015/file/14bfa6bb14875e45bba028a21ed38046-Paper.pdf>


ImageNet-CVPR09
J. Deng, W. Dong, R. Socher, L. J. Li, L. Kai, and F.-F. Li, “Imagenet: A
  large-scale hierarchical image database,” in IEEE Conference on
  Computer Vision and Pattern Recognition (CVPR), 2009, Conference
  Proceedings, pp. 248–255.

DeepLearning-GoogLeNet-CVPR15
C. Szegedy, L. Wei, J. Yangqing, P. Sermanet, S. Reed, D. Anguelov, D. Erhan,
  V. Vanhoucke, and A. Rabinovich, “Going deeper with convolutions,” in
  IEEE Conference on Computer Vision and Pattern Recognition (CVPR),
  2015, Conference Proceedings, pp. 1–9.

UNESCO-RecommendationOpenScience2021
UNESCO, “Unesco recommendation on open science,” 2021. [Online]. Available:
  <https://doi.org/10.54677/MNMH8546>


Tasseology-2008
R. E. Guiley, “Tasseomancy,” in The Encyclopedia of Witches, Witchcraft
  and Wicca.1em plus 0.5em minus 0.4emCheckmark Books, 2008.

Tasseology-TeaLeaves-2016
K. Henriott-Jauw, Tea and Tasseomancy: A Guide to Reading Tea
  Leaves.1em plus 0.5em minus 0.4emCreateSpace Independent
  Publishing Platform, 2016.

Tasseology-TeaLeaves-2014
A. O'Reilly, Tea with Annie, a divinitive guide to Tasseomancy and its
  artistry.1em plus 0.5em minus 0.4emMelbourne, Australia:
  Whiteslaw Press, 2014.

PositiveMood-CreativeProblemSolving-Sage2015
J. Politis and J. C. Houtz, “Effects of positive mood on generative and
  evaluative thinking in creative problem solving,” Sage Open, vol. 5,
  no. 2, p. 2158244015592679, 2015. [Online]. Available:
  <https://journals.sagepub.com/doi/abs/10.1177/2158244015592679>


PositiveMood-CognitiveControl-2011
E. A. Martin and J. G. Kerns, “The influence of positive mood on different
  aspects of cognitive control,” Cognition and Emotion, vol. 25, no. 2,
  pp. 265–279, 2011. [Online]. Available:
  <https://doi.org/10.1080/02699931.2010.491652>


PositiveMood-Cognition-Classification-2010
R. T. Nadler, R. Rabi, and J. P. Minda, “Better mood and better
  performance:learning rule-described categories is enhanced by positive
  mood,” Psychological Science, vol. 21, no. 12, pp. 1770–1776, 2010.
  [Online]. Available:
  <https://journals.sagepub.com/doi/abs/10.1177/0956797610387441>


ImagesMoodSurvey2016
M. K. Uhrig, N. Trautmann, U. Baumgärtner, R.-D. Treede, F. Henrich,
  W. Hiller, and S. Marschall, “Emotion elicitation: A comparison of pictures
  and films,” Frontiers in Psychology, vol. 7, 2016. [Online].
  Available:
  <https://www.frontiersin.org/journals/psychology/articles/10.3389/fpsyg.2016.00180>


ImagesMoodSurvey2008
J. K. Olofsson, S. Nordin, H. Sequeira, and J. Polich, “Affective picture
  processing: An integrative review of erp findings,” Biological
  Psychology, vol. 77, no. 3, pp. 247–265, 2008. [Online]. Available:
  <https://www.sciencedirect.com/science/article/pii/S0301051107001913>


MoodSmilingFace2019
J. Nikitin and A. M. Freund, “The motivational power of the happy face,”
  Brain Sciences, vol. 9, no. 1, p. 6, 2019. [Online]. Available:
  <https://www.mdpi.com/2076-3425/9/1/6>


CognitionStressNegative2024
G. Picciotto and R. A. Fabio, “Does stress induction affect cognitive
  performance or avoidance of cognitive effort?” Stress and Health,
  vol. 40, no. 1, p. e3280, 2024. [Online]. Available:
  <https://onlinelibrary.wiley.com/doi/abs/10.1002/smi.3280>


CognitionStressNegative2023
E. J. Kim and J. J. Kim, “Neurocognitive effects of stress: a metaparadigm
  perspective,” Molecular Psychiatry, vol. 28, no. 7, pp. 2750–2763,
  2023. [Online]. Available: <https://doi.org/10.1038/s41380-023-01986-4>


StressCognitionFeedback2020
M. Paul, C. Bellebaum, M. Ghio, B. Suchan, and O. T. Wolf, “Stress effects on
  learning and feedback-related neural activity depend on feedback delay,”
  Psychophysiology, vol. 57, no. 2, p. e13471, 2020. [Online].
  Available: <https://onlinelibrary.wiley.com/doi/abs/10.1111/psyp.13471>


MoodWeather2008
J. J. A. Denissen, L. Butalid, L. Penke, and M. A. G. van Aken, “The effects
  of weather on daily mood: A multilevel approach,” Emotion, vol. 8,
  no. 5, pp. 662–667, 2008.

MoodWeather1984
E. Howarth and M. S. Hoffman, “A multidimensional approach to the relationship
  between mood and weather,” British Journal of Psychology, vol. 75,
  no. 1, pp. 15–23, 1984. [Online]. Available:
  <https://bpspsychub.onlinelibrary.wiley.com/doi/abs/10.1111/j.2044-8295.1984.tb02785.x>


YOLOv5-2021
Ultralytics, “YOLOv5: A state-of-the-art real-time object detection
  system,” 2021. [Online]. Available: <https://docs.ultralytics.com>


YOLOv5-architecture-2021
——, “Ultralytics YOLOv5 architecture,” 2021. [Online]. Available:
  <https://docs.ultralytics.com/yolov5/tutorials/architecture_description/>






 Paper Title: Human-Machine Co-Adaptation for Robot-Assisted Rehabilitation via Dual-Agent Multiple Model Reinforcement Learning (DAMMRL).




Ge-based Clinopyroxene series: first principles and  experimental local probe study
    João P. Araújo
    August 5, 2024
===================================================================================

empty
empty



























































































§ ABSTRACT 
	This study introduces a novel approach to robot-assisted ankle rehabilitation by proposing a Dual-Agent Multiple Model Reinforcement Learning (DAMMRL) framework, leveraging multiple model adaptive control (MMAC) and co-adaptive control strategies. In robot-assisted rehabilitation, one of the key challenges is modelling human behaviour due to the complexity of human cognition and physiological systems. Traditional single-model approaches often fail to capture the dynamics of human-machine interactions. Our research employs a multiple model strategy, using simple sub-models to approximate complex human responses during rehabilitation tasks, tailored to varying levels of patient incapacity. The proposed system's versatility is demonstrated in real experiments and simulated environments. Feasibility and potential were evaluated with 13 healthy young subjects, yielding promising results that affirm the anticipated benefits of the approach. This study not only introduces a new paradigm for robot-assisted ankle rehabilitation but also opens the way for future research in adaptive, patient-centred therapeutic interventions.






§ INTRODUCTION

Advancements in robotics have significantly enhanced robot-assisted rehabilitation, helping patients regain motor functions through innovative therapies <cit.>. Robotic devices provide precise movement guidance and adjustable resistance, supporting exercises for muscle strengthening and joint flexibility <cit.>. Recent developments focus on improving systems for both upper and lower limb rehabilitation, using technologies like virtual reality and game-like scenarios to make therapy more engaging and effective. These innovations are crucial for better patient outcomes, mimicking natural movements and providing essential feedback to improve motor skills.







Fundamentally, when considering the rehabilitation purpose for different stages of the rehabilitation (e.g., active rehabilitation and passive rehabilitation),  the robot-assisted rehabilitation process can be regarded as a complicated sequential decision-making process <cit.> <cit.> <cit.>, for which the Reinforcement Learning (RL) based approaches are often employed as a powerful tool to perform its optimization process. 
Incorporating reinforcement learning (RL) into rehabilitation also naturally leverages the human feedback loop. The reward prediction error (RPE) theory of dopamine function, rooted in RL, differentiates goal-directed from habitual learning strategies <cit.>. Given the subconscious and individual nature of human learning, personalized evaluation is crucial for optimizing human-machine interfaces (HMI) using RL <cit.> <cit.> <cit.>. Furthermore, Model-Based RL (MBRL) is preferred for human decision-making, aligning with dual process theory and the 'thinking fast and slow' framework <cit.> <cit.> <cit.>.

In robot-assisted rehabilitation, to effectively supervise the adaptation process,  in clinical practice, human-machine co-adaptation is essential through developing mutual learning algorithms <cit.>. To this end, this paper introduces a patient-machine dual-agent decision process that emphasizes human adaptation to the machine, building on our recent research <cit.>. In that work, we proposed a Cooperative Adaptive Markov Decision Process (CAMDP) model to study patient-machine co-adaptation, focusing on the convergence and switching behavior of both agents as an initial step in developing policy adaptation algorithms <cit.>. Additionally, we modified the finite MDP-based reinforcement learning approach to address non-stationarity in multi-agent reinforcement learning (MARL).

The main purpose of introducing the dual-agent MDP model is to enhance the performance and efficiency of human-machine systems through RL-based learning in both online and offline scenarios <cit.>. Learning efficiency is measured by the human's proficiency in controlling the machine. In machine-oriented reinforcement learning, research often includes learning curves that plot optimality (cumulative return) on the y-axis against various measures of time complexity (such as real-world sample complexity, model sample complexity, and computational complexity) on the x-axis. Studies suggest that RL-based reinforcement learning can effectively enhance both cumulative reward and optimality. In this study, we designed reward functions that incorporate desired exercise performances and rehabilitation effect indicators. These functions are tailored to different rehabilitation purposes and stages of the patient, allowing for patient-oriented personalized robot-assisted rehabilitation. 

The scarcity of clinical data and the need for safety and explainability <cit.> make Model-based MARL <cit.> ideal for clinical studies like robot-assisted rehabilitation. It demonstrates advantages in data utilization, human knowledge integration, safety, and explainability <cit.>. State and temporal abstraction can further leverage these benefits.
This is why, in <cit.>, we investigated two agents with finite states and actions described by finite MDPs <cit.> <cit.>.

However, there are many difficulties in using a fully finite MDP model-based multi-agent RL with finite actions to address real application issues, especially in robot-assisted rehabilitation, where both the state and action spaces are often infinite. Additionally, real-time model identification adds computational burdens. In this study, to apply the approach given in <cit.> with finite states and actions described by finite MDPs <cit.> <cit.>, we proposed multiple model approaches used on our previous research <cit.> <cit.>. 



First, we established relatively simple sub-models for the controllers and regulators of the robots, all of which are designable. It is well known, to implement both model training in a simulated environment and real-time control in a real environment, modeling human-related variables and complex robot mechanics is a critical step. For modeling robot mechanics, we translated our SolidWorks Ankle robot model into a Simulink model (see Figure <ref>) and conducted both RL training and real-time testing in a Matlab Simulink and Python integrated structure via serial port wireless communication (e.g., TCP/IP) (see Figure <ref> for the communication between high-level RL training and low-level motor control).



Modeling human behavior is always challenging due to the nonlinear, time-varying nature of human nervous systems. In this study, to model the human agent without detailing the complexities of the human cognitive system, we treat the human as an input-output system. The inputs are various stimuli, including sound, light, electrical signals, and complex interfaces like Virtual Reality (VR) and/or Augmented Reality (AR), which humans can perceive and respond to. The outputs are the human reactions (biofeedback) to these stimuli, which we cam measure using sensors. For example, in our previous research on audio-stimulated bicycle cardiovascular exercise (see <cit.>), we used an input-output-model-based approach to regulate heart rate response along a predetermined reference profile during cycle-ergometer exercises. We designed a system that transmitted commands as auditory biofeedback to adjust exercise intensity via the patient biofeedback system.  Specifically, this method ensures that exercise intensity is effectively modulated by a beep generated by a computer audio system in response to real-time heart rate measurements. 
 By applying input-output modeling and multiple model strategy, we can create multiple sub-models of patient biofeedback, allowing us to individualize the types of biofeedback.




For this study, we developed a visual indicator in a set of digits {0, 1, -1, 2, -2} (see Figure <ref>) to indirectly train the cognitive system using visual stimuli to influence the torque generated by the patient's lower limb musculoskeletal system. This torque can be measured using pressure sensors embedded in the foot pedal, as shown in Figure <ref>. The patient can apply pressure to the foot pedal using either the toes or the heel, thereby affecting the front-end and back-end pressure sensors, respectively. We can build sub-input-output biofeedback models (i.e., visual stimuli as the input and the differential readings of the front and back pressure sensors) according to different rehabilitation stages and exercise locations, among other factors.



By treating the selection of multiple patient biofeedback models and robot sub-controllers as a co-adaptation process between two agents, we can implement a finite action space for both agents. This proposed multiple-model RL (MMRL) based adaptation is notably simpler than fully model-based or model-free RL approaches, streamlining the adaptation process.

In this study, to address the application of an ankle rehabilitation robot,  the multi-model strategy combined parameterized approaches during the RL training and testing procedures.  Specifically, we use neural networks to describe the adaptive sub-model switching policy, accommodating the complex relationship between states (observations) and actions in the rehabilitation process. This approach allows us to implement rehabilitation exercises efficiently without knowing the exact model structure of the sub-model selection logic. Instead, we focus on training the parameters (i.e., the weights of the neural network) to achieve better co-adaptation between human and machine based on dual-agent sub-model matching through optimal switching.



As a medical-related application, robot-based rehabilitation has unique characteristics <cit.> <cit.>. To ensure interpretability, transparency, and avoid overfitting, it is crucial to control model complexity. In the future, we may explore model reduction techniques, such as pruning <cit.> <cit.>, to simplify the complexity of the neural network for the switching logic. This will help reduce model complexity and improve explainability, thereby enhancing acceptance among rehabilitation medical staff and patients.













The principal contributions of this study can be summarized as follows:


	
  * A novel application of AR/VR technology is integrated into rehabilitation exercises, enabling adjustable difficulty levels tailored to the capabilities of stroke patients at various recovery stages.
	
	
  * A multi-model input-output system is employed for the reinforcement learning (RL) framework, facilitating the optimization and ranking of sub-controllers according to value functions. This allows for a sophisticated analysis and decision-making process in real-time during rehabilitation sessions.
	
	
  * The value function is innovatively utilized to modulate the extent of active or passive engagement required from patients during rehabilitation, enhancing the adaptability of the exercises to individual patient needs.
	
	
  * A co-adaptive control strategy is implemented via RL, which dynamically alternates between fixed human input-output submodels and sub-controller models. This strategy enables personalized calibration of the rehabilitation process, determining optimal controller settings or ranking various human model inputs effectively.
	
	
  * The study extends the design of PID sub-controller models and an assortment of human input-output submodels. These designs are rigorously evaluated through experimental trials, providing empirical evidence to support the efficacy of the proposed framework.
	
	
  * This study contributes to the field by introducing a multiple model strategy that significantly reduces the action space, effectively simplifying the complexity of the actor and critic networks for both human and machine agents. Furthermore, the innovative design of the sub-models for each agent incorporates human expert knowledge, enabling a 'human-in-the-loop' approach. This advancement not only streamlines the decision-making process but also ensures that machine actions are closely aligned with human expertise and intuition, enhancing the overall effectiveness and applicability of the system.
	


Overall, this research introduces an advanced dual-agent, multi-model reinforcement learning paradigm for robot-assisted ankle rehabilitation, promising significant strides in personalized, adaptive therapeutic interventions.



§ SETTINGS OF SIMULATION TRAINING ENVIRONMENT AND REAL-TIME TESTING ENVIRONMENT


In this study, the primary rehabilitation device utilized is an ankle rehabilitation robot, as depicted in Figure <ref> and <ref>. Developed at the Jinan Key Lab of Intelligent Rehabilitation Robotics, this robot's pedal is equipped with force sensors that measure the pressure exerted by the toes and heels of the rehabilitation exerciser (Agent_0). These measurements are crucial for designing a controller that drives the pedal based on the differential readings of the front and back pressure sensors, as discussed in the introduction section. The difference in pressure readings from the front and back sensors is amplified by a sub-controller (often a PD controller) to control the pedal motors (for details about the sub-controller or regulators, see Table <ref>).

On the other hand, the machine (Agent_1) provides a control output supplied by its sub-controller (often a PID controller).

For a robot-assisted rehabilitation system, RL training, testing, and analysis can be implemented in three distinct scenarios—real-world, simulated, and abstracted environments. In our previous research <cit.>, the convergence analysis of the CAMDP model was treated in an abstract environment, where state, action, and time are abstractions of real rehabilitation scenarios. In this study, we mainly investigate the real-world and simulated environments.

We briefly introduced the real-world setting of the ankle rehabilitation system. To set up the simulated environment, as discussed in the introduction, we used Simulink/Matlab to model the machine (the rehabilitation robot). The input-output biofeedback model of the patient is also embedded in the Simulink/Matlab model. The simulated environment is used to establish the training environment for the switching networks (actor and critic networks). These trained networks are then applied in real-world experiments via TCP/IP serial port communication. Specifically, within simulated environments, the robot's model using Simulink/SimMechanics is shown in Figure <ref>, and the overall structure for training and testing is depicted in Figure <ref>. The input-output model representing human interaction, from the initiation of movement to the force applied, is also included in Figure <ref>.





 §.§ The overall structure of the system

The proposed block diagram of the multi-agent rehabilitation system is shown in the Figure <ref> and <ref>. In the initial phase, a machine agent and a virtual human agent are separately instantiated. The machine agent serves to govern the dual operating modes of the motor, each mode being associated with an individual sub-controller (here is a Proportional-Integral-Derivative (PID) controller). Similarly, the virtual human agent supervises the intensities of the patient's pedaling, with each intensity controlled by a PD controller. Subsequently, a motion trajectory signal is designated as the desired trajectory for the rehabilitation robot. Upon receiving the control mode from the machine agent, the corresponding PID controller is activated, subsequently regulating the ankle rehabilitation robot's motor torque to facilitate patients' rehabilitation training. Concurrently, the robot's sensors monitor the patient's exertion level, comfort, and modifications in motor mode in real-time, which serve as state/bservation variable inputted into the virtual human agent. The agent generates force guidance, which the patient follows displayed on the screen (the VR as shown in Figure <ref>). The actual pedaling intensity is dynamically adjusted by the sub-PD controller chosen by the virtual human agent, ensuring that the patient performs effective exercise within a comfortable setting.






§ THE KEY COMPONENTS OF DUAL AGENT RL


In this study, we will use dual-agent coadaptive control strategy based on reinforcement learning. In our previous study <cit.>, we proposed a CAMDP model to study the co-adaptation between patient and machine for the analysis of the convergence and switching behavior of the two agents. In <cit.>, we modified the finite MDP model based reinforcement learning to address the non-stationary phenomenon for MARL. Based on the framework in <cit.>, we can construct the similar structure of the two agents.   


To this end, we should well define several key components of the model M=<S_1,   S_2,   A_0,  A_1, R>, the state space (or observation space), action space and reword functions for both Agent_0 (the patient) and Agent_1 (the robot). It should be noted that the Agent_0 for the patient is a generalized concept, which includes all the variables can only be trained rather than designed. On the other hand, the varaibles which can be easily designed and manipulated belong to Agent_1, the intelligent robot or machine.














Specifically, for Agent_0, in simulation, the state/observation includes simulated position (p), trajectory error (e_t), trajectory smoothness (sm_t), previous VR indicator value (v_prev), and motor torque provided by machine (τ_m). The action is the VR indicator (v_VR) associated with different sub-PD controllers, represented by the policy function: 
    v_VR = π_0(p, e_t, sm_t, v_prev, τ_m).
 
In real-time testing, the state/observation are the same as the simulation setting, but the observations are all from the real world. The action remains the selected VR indicator (in a discrete range {-2, -1, 0, 1, 2} as indicated by the HMI as shown in Fig. <ref>) associated with different sub-PD controllers, but applied in the real environment (see Figure <ref>).



For Agent_1, in simulation, the state/observation includes the reference positions (r_p), simulated motor position (p_m), trajectory error (e_t), angular velocity (ω), previous machine action (a_prev), and torque provided by Human (τ_h). The action is the selected sub-PD controller (c_sub-PD), represented by the policy function: 
    c_sub-PD = π_1(r_p, p_m, e_t, ω, a_prev, τ_h).
 
In real-time testing, the state/observation are the same as the simulation setting, but the observations are all from the real world. The action remains the selected sub-PD controller, but applied in the real environment (see Figure <ref>).




























The reward function generally can be structured into two main categories: continuous and enumerable variables. Continuous functions might include the error in tracking a predefined exercise trajectory; the patient's exercise effort quantified by the integrated torque or force they generate; and the assistive energy from the machine, measured by the power output of the machine. Enumerable variable could encompass the patient's emotional state determined through EEG or image-based emotion classification; and the physiological state of the patient estimated from wearable body sensors (e.g., heart rate monitors and wearable IMU sensors) and video cameras.

In this study, the reward function for the robot-assisted ankle rehabilitation system is treated as a continuous variable, encompassing both tracking error and the patient's exercise effort. Different weights can be assigned to these rewards depending on the rehabilitation stage and objectives. For instance, to highlight the patient's activity (i.e., emphasizing active rehabilitation), which is often crucial in the later stages of rehabilitation, we can allocate more weight to the patient's effort. Conversely, in the early stages of rehabilitation, to emphasize the machine's assistive role (i.e., emphasizing passive rehabilitation), we assign more weight to the tracking error. 

As this is a cooperative game, we combine the two agents' reward functions into one by using a weighted summation for both the human and the machine. For a detailed description of the reward function construction, see Subsection <ref>.

This reward function setting is only a specific example. In fact, we can also select a combination of both continuous and categorical variables, especially for patient-related variables such as classified emotional states. Additionally, the weighting of the reward function is flexible and can be adjusted to suit different rehabilitation purposes.



§ THE PRACTICAL STRUCTURE OF MULTIPLE-MODEL RL
























In our previous study <cit.>, we proposed a Cooperative Adaptive Markov Decision Process (CAMDP) model to study the co-adaptation between patient and machine, focusing on the convergence and switching behavior of the two agents. 

While we established models for both agents in simulated environments, in practice, we adopted model-free reinforcement learning (MFRL) strategy due to the lack of an accurate mathematical model structure of the rehabilitation process. Model-free methods allow direct estimation of policy and value functions, adapting better to real-world variations and ensuring robustness.

To simplify the complexity of co-adaptation, we also introduced a multiple-model (MM) strategy, which focuses on optimizing the switching among different sub-models. Given the difficulty in identifying the switching logic structure, we applied neural networks (NNs) to implement this switching mechanism.

In this study, we adopted the MM strategy to leverage human experiences and simplify action spaces. This approach lies between MFRL and model-based RL (MBRL). While MFRL ignores model parameter estimation and MBRL requires exact model parameters, the MM strategy requires only a set of models to complete the RL or co-adaptive procedure.

It should be noted that even in a simulated environment with a model of the real world, model-free reinforcement learning (MFRL) is often used because exact modeling can be too complex or impractical. Model-free methods can generalize better to real-world variations and adapt to dynamics and changes not captured in the simulation, allowing for learning optimal policies directly from interactions.

Therefore, we combined the MM strategy with MFRL, using neural networks to construct the two agents. By optimizing the network weights, we implemented the co-adaptation of the two agents. For a detailed description of the network structures for both agents, see the following subsections.







 §.§ Network Structures of the Two Agents


In both simulation and real environments, the virtual human agent and the machine agent each consist of an actor and a critic. The actor uses a three-layer neural network that takes states/observations as input and generates action probabilities. The machine agent manages the ankle rehabilitation robot's motor torque, ensuring precise motion along predetermined paths. It considers the pedal's reference position, real-time coordinates, rotational velocity, and trajectory deviations. To adapt interactively with the virtual human agent, it integrates previous actions and external forces from the patient. The actor network structure is shown in Figure <ref>.


The fundamental responsibility of the virtual human agent lies in guiding patients through rehabilitation exercises, aiming to maximize their repetitions while ensuring a comfortable environment. Consequently, the agent's state monitoring is intrinsically intertwined with the efficiency and continuity of the motion trajectory, serving as a vital indicator of effective guidance. It is imperative that the agent's observational parameters encompass the precise ankle position of the patient during these exercises, along with the deviation from the predefined reference trajectory for precision assessment. Despite the patient's diligent adherence to the agent's instructions, minor deviations may occasionally occur during the execution of rehabilitative actions. As such, it is essential to incorporate the real-time feedback of the patient's actual actions into the agent's state observation for continuous improvement and adaptation. Furthermore, to facilitate seamless human-machine collaboration, the inclusion of the machine's supportive torque and the patient's preceding actions within the state observation framework is indispensable. The actor network structure of the virtual human agent is shown in Figure <ref>.

In the proposed architecture, the evaluative framework for both the simulated human agent and its real counterpart adheres to a uniform structure. Both entities are modeled using a three-layer neural network architecture, which is primarily tasked with evaluating the value or desirability of a given state. The critic network functions by taking state information as input and computing an estimate of the expected cumulative reward, often referred to as the value function, following the execution of an action. The structure of the critic network is illustrated in Figure <ref>.



 §.§ Details about the construction of reward function
 
The construction of the reward mechanism is of paramount importance, as it directly influences the policy and behavioral decision-making of the virtual human agent during its interactions with the machine counterpart. The primary objective for the machine agent is to facilitate the patients' rehabilitation process through precise motion assistance. To achieve this, the machine's actual movement trajectory should closely align with the pre-established reference path, where precise movement is crucial for effective therapy. Therefore, the squared trajectory error is incorporated as a component of the reward function to penalize deviations from the desired path.

Additionally, maintaining an appropriate speed is crucial to prevent excessive stress and potential damage to both the machine and the patient. For instance, excessive speed can cause machine wear and tear or discomfort for the patient, making effective speed regulation essential. Thus, the reward scheme for the machine agent comprises two principal elements: position tracking reward and speed control reward. These elements ensure that the machine not only follows the desired trajectory but also does so at a safe and effective speed. A possible reward function for the machine  r_machine can be defined as:

    r_machine=σ∑_i=z-k^z(P_i-P_i)^2+β ω_z

where k is the number of state observations; z signifies the current time. σ, β are weights of reward items; P_i is the i-th pedal actual position; P̂_i is the i-th reference position; ω_z is the current pedal angular velocity. From the standpoint of the patient, the key objective is to guarantee a comprehensive ankle rehabilitation process and ensure safety and comfort during the device's utilization. The fundamental purpose of rehabilitation exercises lies in facilitating patients' activities by means of predefined movement pathways. Consequently, the virtual human agent's reward should also consider the precision in tracking the motion position and minimize the angular velocity, which can be the same as r_machine (see equation (<ref>)). Only, simply set the motion tracking reward item r_m as follows:

    r_m=∑_i=z-k+1^z(P_i-P_i)^2

Furthermore, considering the inherent challenge of real-time patient feedback in rehabilitation, it is imperative for us to deduce their emotional state from the available data. In general, it is observed that motion trajectories characterized by uninterrupted movement with minimal acceleration, essentially near-zero acceleration, often evoke a sense of comfort among patients. The comfort-reward item r_c is defined as follows:

    r_c=∑_i=z-k+3^z|P_i+P_i-2-2P_i-1|

To attain the objective of active physical therapy, it is imperative for patients to engage proactively with the device by repetitively exerting force voluntarily to enhance recovery outcomes. Consequently, the virtual human agent plays a pivotal role in guiding patients towards frequent force application. We assess the reward of active movement through quantifying the volatility of actor's action. Despite the virtual human agent's capacity to offer guidance, potential discrepancies in patient compliance or reduced engagement necessitate the agent's adaptability to individual behavioral traits. To address these circumstances, our reward algorithm incorporates a regulatory component that acknowledges such deviations. In instances where patients deviate from instructions or demonstrate inadequate activation, the virtual human agent refrains from mandating increased exertions. Instead, the system dynamically adjusts to the patient's current capability, transitioning seamlessly from active to passive rehabilitation mode. The effort-reward function r_e is defined as follows:

    r_e=E/k-2∑_i=z-k+1^z-1(A_i-1/k∑_i=z-k+1^zA_i)^2

where A_i is the i-th action from the virtual human agent; E is human effort coefficient; if the last action is unequal to the current action, E is 1, otherwise, E is 0. Ultimately, we amalgamate these rewards and modulate the significance of each by assigning appropriate weights. Consequently, the reward function r_human for the virtual human agent is formulated as follows:

    r_human=μ r_m+κ r_c+ρ r_e

where μ, κ, ρ are weights of reward items.

Finally, if we do not punish the pedal angular velocity, considering the similarity of equations (<ref>) and (<ref>), as well as the components of (<ref>), which align with the cooperative rehabilitation goal, we can, in practice, simply use equation (<ref>) as the common reward function for both agents.





§ TRAINING THE AGENTS


During the concurrent training of machine and virtual human agents, real patients are excluded to prevent early adaptation issues and control strategy failures. Offline model training ensures optimal performance without real-world interference. We use a virtual ankle joint model, assuming the virtual human's output mirrors expected patient behavior. Controlled random noise simulates performance discrepancies, ensuring system efficacy and practical utility. 

The depicted schematic representation of training the multi-agent rehabilitation system is presented in the Figure <ref>. The detailed training protocol proceeds as follows.

The training strategies for virtual agent intelligence and machine agent intelligence are identical. Initially, the actor engages in interactions with the environment to accumulate an extensive dataset, which is subsequently stored in the experience replay buffer. Given the current state S_t, the actor computes the probability distribution of actions, from which a random action a_t is chosen. Following the execution of this action, the system transitions to the subsequent state S_t+1, accompanied by a corresponding reward r_t. The variable t signifies the current time. By calculating the return of each interaction, the conventional On-Policy strategy employs real-time rewards as proxies for long-term returns in gradient updates, often resulting in substantial algorithmic fluctuations. To mitigate these fluctuations, the Actor-Critic approach is adopted. This strategy involves utilizing a model to estimate the state values, thereby reducing the variance during optimization. Consequently, the advantage function Q_t is derived, leveraging the benefits of both policy evaluation and control in enhancing the learning process.


    Q_t = ∑_i=t^N_e-1γ^i-t (r_i + γ V(S_i+1, ϕ) - V(S_i, ϕ)) 6


where N_e is the length of the experience buffer; γ is the discount factor; V(S_i, ϕ) is the critic network with the parameter values ϕ and the i-th state observation input. Then we can obtain the return R_t:


    R_t = Q_t + V(S_t, ϕ) 7


During the update process, a batch of data is initially retrieved from the experience repository, where each datum is associated with the advantage function and cumulative return values. The critic's principal function is to forecast the overall score subsequent to taking an action in a given state. The predicted score for the subsequent time step should exhibit a deviation, equating to the immediate single-step reward garnered at the current time point. The critic network aims to minimize the discrepancy between the predicted cumulative return and the actual return, incorporating the advantage function to optimize the estimation accuracy and stabilize learning. Consequently, grounded in these theoretical foundations, the loss function L_c(ϕ) of the critic network can be formally defined as follows:


    L_c(ϕ) = 1/2B∑_n=1^B (R_n - V(S_n, ϕ))^2 8


where B is the batch size. The principal component of the actor's loss is computed as the ratio between the novel policy and its preceding counterpart. To maintain training stability, a clipping mechanism is employed. First, a clipping threshold is set, establishing an upper bound at 1+ε and a lower bound at 1-ε. Whenever the ratio between the updated actor and its initial state surpasses these bounds, the actor's loss is determined by the nearest predetermined constraint. This practice restricts the policy's adaptation to occur exclusively along the gradient vector confined within the established boundaries, preventing direct adjustments based on the raw ratio. This strategy effectively prevents drastic policy fluctuations over brief periods, thereby minimizing the likelihood of instabilities or oscillatory behavior during the learning process. The clipping mechanism enables the algorithm to approximate an optimal update at each iteration while preserving the practical constraints on policy revisions. Consequently, even if the newly proposed policy occasionally exhibits superior return prospects, it reverts to a secure boundary if it exceeds the allowable update parameters, thereby safeguarding the integrity of the current policy performance. Formally, the actor network's loss function L_a(θ) can be mathematically expressed as:


    L_a(θ) = 1/B∑_n=1^B(   -min(T(a_n | S_n, θ)/T(a_n | S_n, θ̂)· Q_n, clip_n(θ) · Q_n) 
       + α K_n(θ, S_n)) 9



    clip_n(θ) = max(min(T(a_n | S_n, θ)/T(a_n | S_n, θ̂), 1+ε), 1-ε) 10


where T(a_n | S_n, θ) is the probability of taking the n-th action a_n when in state S_n, given the updated policy parameters θ in the batch set; θ̂ is the previous policy parameters before current updating epoch; ε is the clip factor; α is the entropy loss weight factor; P_n(θ, S_n) is the entropy loss item. In the realm of reinforcement learning, an effective strategy necessitates not only the optimization of expected returns but also a judicious balance between exploitation and exploration to uncover rewarding actions. Entropy, as a quantitative indicator, gauges the inherent randomness or uncertainty within a policy, with higher values reflecting a greater propensity for exploration. The Proximal Policy Optimization (PPO) framework integrates a regularization mechanism, which promotes the maintenance of a stable exploration level concurrently with the objective of maximizing rewards. This strategy safeguards the policy against premature convergence towards suboptimal local optima. Additionally, entropy plays a crucial role in balancing immediate rewards with long-term exploration benefits. Overly narrow policies risk disregarding alternative, potentially more advantageous long-term scenarios. By augmenting entropy, PPO actively fosters policy diversity, thereby enhancing overall performance over extended periods. Moreover, high-entropy policies serve as a safeguard against overfitting, an issue that arises from excessive specificity in the policy. This allows the policy to exhibit enhanced generalizability across various environments. As a result, the entropy loss term, which embodies this balance between exploration and exploitation, assumes a pivotal role in the formulation of the PPO algorithm's objective function. Thus, the entropy loss item K_n(θ, S_n) can be defined:


    K_n(θ, S_n) = -∑_l=1^Y T(a_l | S_n, θ) ln T(a_l | S_n, θ) 11


where Y is the number of actions. T(a_l | S_n, θ) is the probability of taking the l-th action a_l when in state S_n following the current policy. In compliance with the established reward mechanism and model update procedure, the patient engages in interactive training with both the virtual human agent and the machine agent. Gradually, the patient acclimates to the mode transition rules of the rehabilitation robot during therapy, prompting the device to adapt its approach dynamically based on the patient's ongoing performance. This continuous exchange facilitates concurrent improvement for both parties, ultimately resulting in a synergistic adaptation between the participant and the rehabilitation device.



§ THE ARRANGEMENTS OF THE MULTIPLE MODELS FOR BOTH HUMAN AND MACHINE AGENTS


One of the major strategies of this study is the use of multiple model techniques. To effectively integrate this with the RL-based adaptation procedure, we first merged the multiple models into the action spaces of the two agents. This greatly reduced the complexity of the policy networks for both agents. Furthermore, we limited the size of the sub-controllers by carefully selecting suitable submodels to implement online model switching. This approach significantly minimizes the adverse effects of transient behavior due to model switching during both training and testing.

For the design of the sub-controllers and the sub-models, we used classical control design strategies, such as PD and PID control with different control parameters, to implement fine-tuning and co-adaptation. As we have built both simulation and real environments for training and testing, we can train the RL networks offline with different reward functions to serve various training purposes, such as fully passive rehabilitation exercise and semi-active rehabilitation, before real experimental testing. The details of the designed sub-controllers, reward functions, and achieved training results (value functions) are summarized in Table <ref>.





The table <ref> provides a comprehensive summary of the designed sub-controllers, reward functions, and achieved training results (value functions). Each row in the table represents a different configuration of reward functions and control parameters for both human and machine sub-controllers. The configurations are numbered from 1 to 8 for easy reference.



 §.§ Reward Functions

The reward functions used in the study are denoted by different combinations of r_m, r_c, and r_e. These terms represent various components of the reward, such as machine effort, cooperative behavior, and error minimization, respectively. Two main types of reward functions are used:

	
  * r_m + r_c + 5r_e
	
  * r_m + 8r_c + r_e

These reward functions are designed to balance the trade-offs between different aspects of the rehabilitation task.



 §.§ Sub-Controllers

For each reward function, different sub-controller configurations are tested. The sub-controllers for the human agents are implemented using PD (Proportional-Derivative) controllers, while the machine agents use PID (Proportional-Integral-Derivative) controllers. The parameters for these controllers are given in the format:

	
  * PD Controller: [K_p, K_d]
	
  * PID Controller: [K_p, K_i, K_d]




 §.§ Settings and Parameters


	
  * Setting 1: Uses the reward function r_m + r_c + 5r_e. The human sub-controller 1 is configured with parameters [30, 0.2], and sub-controller 2 with [15, 0.1]. The machine sub-controller 1 uses [24, 2.4, 24], and sub-controller 2 uses [12, 1.2, 12]. This Setting achieves a value function of 2124.89.
	
  * Setting 2: Maintains the same reward function but with different machine sub-controller parameters: [12, 1.2, 12] and [6, 0.6, 6]. The value function achieved is 1940.50.
	
  * Setting 3: Keeps the reward function the same but changes the human sub-controller parameters to [5, 0.1] and [2.5, 0.05], achieving a value function of 2617.20.
	
  * Setting 4: Similar to Setting 3 but with machine sub-controllers using [12, 1.2, 12] and [6, 0.6, 6], achieving a value function of 2738.69.
	
  * Setting 5: Uses the reward function r_m + 8r_c + r_e. The human sub-controllers use the same parameters as Setting 1, but the machine sub-controllers also use [24, 2.4, 24] and [12, 1.2, 12], resulting in a value function of 1272.84.
	
  * Setting 6: Similar to Setting 2 but with the new reward function, achieving a value function of 908.92.
	
  * Setting 7: Similar to Setting 3 but with the new reward function, achieving a value function of 460.14.
	
  * Setting 8: Similar to Setting 4 but with the new reward function, achieving a value function of 1626.40.




 §.§ Analysis

From the table, it is evident that different Settings of sub-controllers and reward functions lead to varied performance outcomes, as indicated by the value functions. Settings 3 and 4, using the reward function r_m + r_c + 5r_e, tend to achieve higher value functions, suggesting better performance in terms of the defined reward components. On the other hand, Settings with the reward function r_m + 8r_c + r_e generally achieve lower value functions, indicating a potential trade-off between different aspects of the rehabilitation goals.

These findings highlight the importance of carefully selecting and tuning both the control parameters and reward functions to achieve optimal performance in cooperative rehabilitation tasks. The results demonstrate that by fine-tuning the controllers and appropriately defining the reward structure, it is possible to significantly enhance the efficiency and effectiveness of the rehabilitation process.
















§ THE TRAINING AND EXPERIMENTAL PROCEDURE


This study implements reinforcement learning-supervised rehabilitation. The supervised rehabilitation process is divided into several steps for clarity:

 
	
  * Step 0: Utilise Simulink robot models and pre-experimental data to identify a set of simple sub-models or controllers for various circumstances, along with input-output models for patient biofeedback involved in the feedback loop.
	
  * Step 1: Employ the Actor-Critic network to train the two agents within the Matlab/Simulink environment, as illustrated in Figure <ref>, until the training converges.
	
  * Step 2: Conduct real-world experiments and use VR/AR (Human-Machine Interface (HMI)) to provide stimuli to the patients.
	
  * Step 3: If the exercise effort does not meet the design specifications, adjust actions based on the experimental data to re-model the environment and redesign the controllers. This involves focusing on the modeling of the patient's biofeedback system, treated as an input-output system with the HMI indicator as the input and the response force as the output. Additionally, monitor the patient's condition using wearable sensor-based measurements, taking into account the patient's adaptation to the HMI indicator or the machine.


The overall system will be iteratively improved to achieve co-adaptation between the patient and the intelligent robot.




































































§ EXPERIMENTS









According to the proposed procedure, we recruited 13 healthy young subjects, with details described in Table <ref>. The experiment was approved by Shandong First Medical University under document number "R202402280067". The subjects were familiarised with the experimental procedure, which involved only the testing of the rehabilitation exercise, as the training of the four neural networks had been completed during simulation. The training results are shown in Table <ref>.

Based on the training results, we followed Procedure <ref> and asked the subjects to perform rehabilitation under four different settings.

According to the pre-trained results shown in Table <ref> for Subject 1, for the reward function r_m + r_c + 5r_e (encouraging patients' active participation), we selected the sub-controller settings with the lowest value (Setting 2) and the highest value (Setting 4). For the reward function r_m + 8r_c + r_e (encouraging robot assistance, resembling a passive rehabilitation mode), we selected the sub-controller settings with the lowest value (Setting 6) and the highest value (Setting 8) to complete the rehabilitation exercise.






























§ RESULTS AND DISCUSSIONS









The detailed experimental results are shown in Figures <ref> to <ref>. To investigate the effectiveness of the reward function selection vs the testing outcomes, the testing results of Subject 1 are presented in Figures <ref> to <ref> as the patient input-output biofeedback model used in the simulation environment was based on Subject 1. These figures clearly indicate that the rehabilitation effects can be achieved based on the specifically designed reward functions, demonstrating the desired consistency with the simulated values (See Table <ref>).



Specifically, in Figures <ref> and <ref> (corresponding to Settings 2 and 4 of Table <ref> respectively), it is evident that Subject 1 actively participated in the exercise, as indicated by the higher level of human action (yellow marked) because the designed reward function is encouraging the patients' active participation. On the other hand, the human action in Figures <ref> and <ref> (corresponding to Settings 6 and 8 respectively) is lower because the designed reward function for these two settings is more emphasize the tracking accuracy rather than patient's participation. We can also see this results via MSE value: the human action MSE for setting 2 (Figure <ref>) and setting 4 (Figure <ref>) are 502.25 and 468.17 respectively. They are higher than those for Setting 6 (Figure <ref>) and Setting 8 (Figure <ref>) (See Table <ref>).

Furthermore, in Figures <ref> and <ref> (corresponding to Settings 2 and 4), it is evident that the system has much bigger tracking error in the exercise because the designed reward function is encouraging the patients' active participation rather than tracking accuracy. On the other hand, the tracking accuracy in Figures <ref> and <ref> (corresponding to Settings 6 and 8 respectively) is much lower because the designed reward function for these two settings is more emphasize the tracking accuracy rather than patient's participation. We can also see this results via MSE value: the MSE for tracking error of setting 2 (Figure <ref>) and setting 4 (Figure <ref>)  are higher than those for Setting 6 (Figure <ref>) and Setting 8 (Figure <ref>) (See Table <ref>).

At the same time, we should also pointed out that under the same reward function, in training, Setting 4  achieved a value of 2738.69, which is significantly higher than the value (1940.50) achieved by setting 2. Consistently, in experiments, the overall tracking error for setting 2 (Figure <ref>) is much higher than that in Setting 4 (Figure <ref>).

Similarly, under the same reward function (more emphasize on tracking accuracy), in simulation, setting 8 (Figure <ref>) achieved a value of 1626.40, compared to 908.92 for setting 6 (Figure <ref>). Consistently, in testing, the overall tracking error for setting 8 (Figure <ref>) is lower than that for setting 6 (Figure <ref>). 

Based on this analysis, we can conclude that for Subject 1, the training  values achieved for different settings is consistent with the training results in the simulation environment, as shown in Table <ref>. 









For other subjects, the overall consistency between simulation and testing is similar to that of Subject 1. Particularly for the reward function r_m + 8r_c + r_e (encouraging robot assistance, resembling a passive rehabilitation mode), which corresponds to settings 5-8, the experimental results for different subjects are quite similar, as the effects of the patient are greatly reduced (see Figures <ref>-<ref> for details).

However, for the reward function r_m + r_c + 5r_e (encouraging patients' active participation), which corresponds to settings 1-4, if the set of sub-controllers is not adjusted, different subjects may achieve varying training results. To observe intra-subject differences, consider Subject 13's experimental results for setting 2 (Figure <ref>) and setting 4 (Figure <ref>). Comparing these figures with Figures <ref> and <ref> for Subject 1, the differences are apparent. The actions from different patients can vary significantly (for setting 2, Subject 13 could not complete the exercise due to a much stronger biofeedback reaction compared to Subject 1).























































































































































































































































§ CONCLUSION



In conclusion, this study addresses the challenges in robot-assisted ankle rehabilitation by introducing a Dual-Agent Multiple Model Reinforcement Learning (DAMMRL) framework, leveraging multiple model adaptive control (MMAC) and co-adaptive control strategies. Traditional single-model approaches have struggled to accurately model the complexities of human-machine interactions due to the dynamic and non-linear nature of human behaviour. By employing a multiple model strategy, we approximate complex human responses using a suite of simple sub-models tailored to varying levels of patient incapacity, thus overcoming the limitations posed by the curse of dimensionality.

The proposed framework's efficacy is demonstrated across real-world and simulated environments, showcasing its robustness and versatility. Rigorous evaluation with 13 healthy young subjects yielded promising results, affirming the anticipated benefits of this approach. This research not only introduces a novel paradigm for robot-assisted ankle rehabilitation but also sets the foundation for future studies focusing on adaptive, patient-centred therapeutic interventions, thereby enhancing the effectiveness and personalisation of rehabilitation processes through advanced reinforcement learning techniques.

IEEEtran




 Paper Title: Coverage Path Planning For Minimizing Expected Time to Search For an Object With Continuous Sensing.











Linh Nguyen



Stony Brook University, Stony Brook NY 11794, USA
 linh.nguyen.1@stonybrook.edu

Coverage Path Planning For Minimizing Expected Time to Search For an Object With Continuous Sensing
    Linh Nguyen0009-0009-3518-929X
    August 5, 2024
===================================================================================================



§ ABSTRACT 
In this paper, we present several results of both theoretical as well as practical interests. First, we propose the quota lawn mowing problem, an extension of the classic lawn mowing problem in computational geometry, as follows: given a quota of coverage, compute the shortest lawn mowing route to achieve said quota. We give constant-factor approximations for the quota lawn mowing problem.

Second, we investigate the expected detection time minimization problem in geometric coverage path planning with local, continuous sensory information. We provide the first approximation algorithm with provable error bounds with pseudopolynomial running time. Our ideas also extend to another search mechanism, namely visibility-based search, which is related to the watchman route problem. We complement our theoretical analysis with some simple but effective heuristics for finding an object in minimum expected time, on which we provide simulation results. 








§ INTRODUCTION

In this work, we study search and path planning problems in geometric domains. Consider, for example, a remote-controlled camera inspecting a manufactured component for porosities, or a submarine using radar/sonar to search for a missing object. We model this search, where the agent needs to be in proximity to the target for the sensor to detect it, by the lawn mowing problem, a classic computational geometry problem posed by Arkin, Fekete, and Mitchell in <cit.>. Given a planar region, compute a minimum-length route along which a circle or square (“mower”) can be slid so that every point in the region is “mowed”. The problem was shown to be NP-hard even for a simple polygon, and several approximation algorithms were also given.


The equivalence of the lawn mowing problem for visibility-based search is the watchman route problem, introduced by Chin and Ntafos <cit.>. Given a polygon, the watchman route problem asks for a minimum-length route that sees the whole polygon (where “seeing” means establishing a line of vision that lies entirely within the polygon). 
Another related problem, which shares a similar detection mechanism with the lawn mowing problem is the honey-pot search problem <cit.>, in which a searcher must maximize the probability of finding a target within the given time budget.

Both the lawn mowing problem and the watchman route problem, when stated as search problems, are concerned with the worst-case guarantee, as are many previous works in coverage path planning, see the surveys <cit.>. We study the average-case, i.e., we seek to optimize expected duration of the search. Several studies have looked at minimizing expected detection time in a graph. In <cit.>, Bernman, Ianovsky and Krass proposed an exponential-time dynamic programming algorithm for finding a uniformly distributed target, which Teller, Zofi and Kaspi later extended to non-uniform distribution while also presenting a branch-and-bound algorithm along with several heuristics <cit.>.  Sarmiento, Murrieta and Hutchinson were the first to investigate the expected detection time in visibility-based search <cit.>. They proposed a heuristic that selects a set of points that collectively see the whole polygon (known as “guards”) and visits those points in an ordering resulting from minimization of a utility function.

The problem of minimizing expected detection time is also related to the minimum latency problem (also referred to in the literature as the traveling repairman problem). Given some nodes in a graph or some points in the plane, compute a route that minimizes the average delay of all the nodes. Several approximation algorithms are known <cit.>, including a polynomial-time approximation scheme <cit.>. Our problem is a continuous minimum-latency type problem. 

Most search problems involving path planning in a geometric domain, or even a discrete graph theoretic domain are known to be NP-hard <cit.>. Optimal search theory is an overarching field of research that arises in numerous practical applications, including but not limited to: surveillance, robotics, search-and-rescue, etc. Depending on the task, perception, and target modeling, along with the objective function to be optimized, many different variations have been proposed and studied. We refer the reader to the surveys <cit.> for a comprehensive review.

We provide several novel results on optimal 
 search in a geometric setting:

    
  * For the quota lawn mowing problem, we provide theoretical approximation algorithms, the running time of which depends on the approximation algorithm for quota-TSP/MST involved.
    
  * We prove that minimizing the expected detection time is NP-hard in a simple polygon for both search mechanisms, lawn mowing and visibility-based search. Previously, only hardness in polygons with holes was known <cit.>.
    
  * We provide the first pseudopolynomial-time approximation algorithm with provable error bounds for minimizing the expected detection time for both search mechanisms.
    




§ PRELIMINARIES

We model the area to be searched by a closed planar region R, a subset of ℝ^2. If R is connected and the boundary of R, ∂ R, consists of a finite number of non-crossing line segments, then R is a polygonal region, or synonymously, a polygon with holes. If R has no holes, i.e., R is simply connected, then we call R a simple polygon. We assume R is a polygon with holes with n vertices, though our results can be generalized to more general regions (with regular curved arcs on the boundary or multiple connected components).

We are given a pointwise robot/cutter. Throughout this paper, we shall use the term “cutter” when discussing the lawn mowing problem and the term “robot” when discussing searching. Assume that the operational area of the robot/cutter, χ, is either an axis-aligned square or a circle centered on it. Without loss of generality, we can scale R so that χ is a unit square (side length 1) or a unit circle (radius 1). If the robot/cutter traverses a trajectory γ, the area of coverage, denoted C(γ), is the Minkowski sum of γ with the operational area: C(γ) = (γ + χ) ∩ R. Note that, in the lawn mowing problem, γ need not stay within R. See Figure <ref> for an illustration.



We use |·| to denote Euclidean measure of geometric objects (e.g., length or area), as well as cardinality of finite sets, the meaning should be clear from the context. First, we consider the following problem:


  
The quota lawn mowing problemGiven an area quota 0 ≤ A ≤ |R|, compute the shortest tour (cycle) γ such that |C(γ)| ≥ A.

This can be seen as a generalization of the lawn mowing problem, which is NP-hard even if the input region is a simple polygon <cit.>. Moreover, the lawn mowing problem is algebraically hard <cit.>. The same hardness for the quota lawn mowing problem follows naturally.

Next, we consider some geometric optimal search problems. Suppose there is a target located within R. We assume that the location of the target is uniformly distributed, though our results can be easily adapted to non-uniform distributions, that is, when we have prior knowledge of some regions more likely to contain the target, by simply replacing area with probability measure. A robot, given a starting point s and equipped with a sensor, looks for the target. Think of a camera scanning a manufactured part for a small dent, or a submarine using radar/sonar to look for a missing ship/airplane on the ocean floor.

If the sensor can detect the target within a circle or square centered around the robot, we call it the lawn mowing search mechanism. If the sensor is visibility-based with infinite range, we have the classic watchman route problem (in which the geometry of the region R not only limits visibility but also movements), which we refer to as visibility-based search mechanism. 

The lawn mowing problem and the watchman route problem are concerned with the worst-case scenario of the search process, where the objective is to minimize the distance traveled to guarantee detection of the target, i.e. every point in the domain must be covered. The quota lawn mowing problem introduced earlier, can be interpreted as that of computing a minimum route to guarantee a certain probability of detection. In many applications, we are also interested in an average-case analysis, thus we study the expected detection time as the objective function to optimize. Suppose the robot follows a trajectory γ such C(γ) = R. We define the random variable T to be the time at which the target is detected by the robot. We assume that the robot travels at a constant speed of 1, without loss of generality, so that distance and time are equivalent. Denote by C(γ, t) the partial area of coverage accumulated by the robot by time t as it travels along γ (note that C(γ, |γ|) = C(γ) = R). Then the probability of detection at time t is equal to the fraction of area covered: Pr_γ(T ≤ t) = |C(γ,t)|/|R|.
Thus, since T > 0, the expected value of T can be written as

    E[T|γ]    = ∫_0^∞Pr_γ(T > t)dt = ∫_0^∞(1 - |C(γ,t)|/|R|)dt.




  
The expected detection time minimization problemCompute a route γ that minimizes E[T|γ].







    




§ LAWN MOWING PROBLEM WITH A QUOTA

We begin by assuming the operational area of the cutter, χ, is a closed unit square. As the cutter moves, χ translates but does not rotate.

A pixel is a closed unit square in ℝ^2. We denote by 𝒫 the set of pixels whose vertices are integer grid points that have a non-empty intersection with R. If the cutter is given a starting point s, the grid inducing 𝒫 is shifted if necessary so that s is at the center of a pixel. Denote by 𝒢 the dual grid graph of 𝒫. Each node in 𝒢 corresponds to the center point of a pixel in 𝒫, two nodes are adjacent if their pixels share a side edge (Figure <ref>). Let N = |𝒫|, then we can identify 𝒢 and 𝒫 in O(N + nlog n) from a description of R as an n-vertex polygonal domain.

We consider the input complexity to depend on the magnitude (the bit complexity) of the coordinates of the vertices of R. This allows for algorithms whose running time is pseudopolynomial (depending on the input size as well as magnitude of the input). This is because even for a relatively combinatorially simple region, a quota lawn mowing tour can have a very large number of turns. This was first observed in <cit.>.



Let γ^* denote an optimal lawn mowing tour for a given quota of coverage A. To approximate γ^*, we require the following lemma.



    For a unit square cutter, there exists a tour γ_𝒢 whose vertices are a subset of the vertices of 𝒢 such that |γ_𝒢| ≤ O(1)|γ^*| and C(γ^*) ⊆ C(γ_𝒢).


    Notice that γ^* is necessarily the shortest (fully covering) lawn mowing tour of C(γ^*), a subregion of R. According to <cit.>
    
   
        
  (i) If the cutter is limited to rectilinear motion (γ^* consists of only axis-aligned segments), there exists γ_𝒢 within 𝒢 of length no greater than 3|γ^*| covering C(γ^*).
        
  (ii) If the cutter can translate arbitrarily, we augment 𝒢 with diagonals: join two center points with a diagonal (45 degrees slope) if they are separated by distance √(2). There exists γ_𝒢 within the diagonal-augmented grid of length no greater than 6/√(2 + √(2))|γ^*| covering C(γ^*).
    
    When |γ^*| is small, we can employ the strategy for short covering tours in <cit.>, the details of which we omit in this version.
    ▪

An algorithm that computes an optimal (quota) lawn mowing tour will generally need to select turning points from a continuum rather than a discrete, finite set. This presents significant computational challenges (recall the algebraic hardness of the problem). Lemma <ref> establishes that, for the purpose of constant-factor approximations, it suffices to discretize the problem into a grid graph.

 For each node p of 𝒢, the center point of the pixel dual to p, let r(p) be the area of the pixel that lies inside R, i.e., r(p) = |p∩ R|. This allows us to treat the lawn mowing problem with a quota as an instance of the quota TSP:

 

  
The quota TSP Find the shortest tour through a subset of pixel center points such that the total reward of the tour is no smaller than 
 A. 
 
 By Lemma <ref>, a c-approximation to the quota TSP yields a 3c-approximation in the case of rectilinear motion and a 6c/√(2 + √(2))-approximation in the case of arbitrary motion, to the quota lawn mowing problem.

 We can approximate the quota TSP problem with a factor of 5 using the methods in <cit.> with polynomial (in N) running time. Further, we can achieve a factor of (1 + ε) for any ε > 0 by adapting the methods for geometric k-TSP (Arora <cit.> or Mitchell <cit.>), a special case of quota TSP where the reward of every point is unary and the quota is k as follows:
 
     
  (1) Scale the rewards and quota to be integers. Specifically, let M = 10^D, where D is the maximum number of decimal digits used to represent the fraction part of any r(p) or A. Then M = Dlog_2(10) = O(B), where B is the bit complexity of the problem instance.
     
     We set A:=MA and r(p):=Mr(p) for every p∈𝒫.
     
  (2) For each p, create r(p)-many duplicates of p at the same location as p (distance 0 away from p). This create an O(NM) = O(NB) instance of k-TSP.
     
  (3) Approximate the optimal k-TSP tour for the instance created with k=A.
 
 The running time of this approach is (NB)^O(1/ε).
 
 Similarly, we can use the same transformation to the k-MST problem: Find the minimum cost tree spanning at least k vertices. Doubling an α-approximate tree (traversing each edge twice in opposite directions) gives a 2α-approximate tour. Several approximations with small factors are known for the k-MST problem, the current best factor is 2 for general graphs <cit.> and (1 + ε) for points in the plane <cit.>.
 

  
Circular cutter If the cutter is a circle, rather than square pixels, we consider hexagonal pixels, each of diameter 2. We join two center points by an edge if their respective hexagons share part of their boundary, creating a triangular grid graph (Figure <ref>).


A constant-factor approximation to an optimal quota lawn mowing tour γ^*, can once again be found within the triangular grid graph. Specifically, one no longer than 2√(3)|γ^*| (see <cit.>). The remainder of the approximation method follows through like the case with square cutter.



§ MINIMIZING EXPECTED TIME TO SEARCH FOR AN OBJECT



 §.§ Lawn mowing search mechanism

First, we rewrite the formula for the expected value of T, the amount of time (or distance, interchangably, because we assume the robot travels at a fixed speed of 1) before the target is detected by the robot as follows:

    E[T|γ]    = ∫_0^∞(1 - |C(γ,t)|/|R|)dt= 1/|R|∫_0^∞(∫_x∉ C(γ, t)dx)dt.

For the purpose of optimizing expected detection time in a given region R, we can drop the denominator |R|. 
Notice that for each infinitesimal volume element dx ∈ R, the integral above is taken over the region dx ∈ R ∖ C(γ,t). Hence, we can change the order of integration to be

    ∫_x∈ R(∫_0^t:x∉ C(γ,t)dt)dx=∫_x∈ Rt_γ(x)dx.

where t_γ(x) is the time when the accumulated area of coverage of the robot first contains x when traveling along γ. It may be more intuitive to consider this formula in the discrete case, where the location of the target is given as a uniform distribution on m discrete points x_1, …, x_m. Suppose γ visits those points in that order, then

    E[T|γ]    = ∑_i=1^mt_γ(x_i)Pr(target is found when γ reaches x_i)= 1/m∑_i=1^mt_γ(x_i).

In fact, t_γ(x_i) is known as the latency of x_i.


    Minimizing E[T|γ] is NP-hard, even in a simple polygon.
    



    Our reduction is from the Minimum Latency problem: Given n points p_1, …, p_n on the Euclidean plane, compute a tour γ that minimizes ∑_i=1^nt_γ(p_i), where t_γ(p_i) is the distance traveled before first visiting p_i. The Minixmum Latency problem is known to be as hard as the Euclidean TSP <cit.>. Our reduction works for both rectilinear and unrestricted movement, square and circular searcher.
    
    Given a Minimum Latency problem instance, scale the instance so that the side length (or radius) of the area of coverage of the robot is ε infinitesimally small. We construct a simple polygon P as shown in Figure <ref>. First, interconnect the points by a tree (e.g., the minimum spanning tree). Surround each point with a small square room of side length ε. 
    
    
    Recall that in the lawn mowing search mechanism, the movement of the robot is not constrained to stay within P. Consequently, an expected detection time minimizing tour prioritizes the rooms before attempting to cover all the area of the hallways. Such a tour must visit the rooms in the order that a latency-minimizing tour visits the corresponding points. ▪



  §.§.§ Approximation algorithm
 We give an approximation algorithm for minimizing E[T|γ], utilizing the solution to the quota lawn mowing problem as a subroutine. In the reduction from the quota lawn mowing problem to the k-TSP, we multiplied the area of each pixel with M = 10^D, where D is the decimal precision, thus effectively multiplied the area of R by M. Recall that M = O(B), where B is the bit complexity.

For any A∈{1, 2, …, M|R|}, let L(A) be length of the shortest tour that covers an area of A, which means a c-approximation algorithm would  a tour no longer than cL(A) for area quota A. For j= 1, …, O(log N) (suppose N > 1, else the problem is trivial), denote by A_j the maximum value such that L(A_j) ≤ 2^j. For j= 1, …, O(log N), we can find A_j, the maximum area quota for which the c-approximation returns a tour no longer than c2^j, by a binary search on {1, 2, …, M|R|} as the input quota to the c-approximation. Clearly, A_j ≤A_j.

We state our approximation algorithm for finding a route that minimizes expected detection time as follows: For j = 1, …, O(log N), follow the tour (returned by the c-approximation) associated with area quota A_j, return to the starting point before each increment of j. We denote this tour by γ.



  §.§.§ Running time
 The running time of the above approximation algorithm is the running of the c-approximation to the quota lawn mowing problem (which depends on the quota TSP/MST or k-TSP/MST heuristic employed) multiplied by O(log N)O(log M|R|) = O(log N(B + log N)).



  §.§.§ Analyzing the approximation factor

Let γ^* be an expected detection time optimal tour, and let Pr_γ^*(T > t) = f_γ^*(t). Specifically, f_γ^*(t) is the uncovered fraction of area after the robot follows along γ^* for a time/distance of t. Note that f_γ^*(t) is continuous and monotone (decreasing).

We show another way to compute E[T|γ^*], which will be helpful in our analysis of the approximation factor. Draw the graph of f_γ^*(t) on the Cartesian plane, we wish to compute the area bounded by its curve and the 2 axes. If we switch the 2 axes and consider t a function of p, the area under the curve stays the same. For any 0 ≤ 1 ≤ p, define the function f_γ^*'(p) = t, where t is the minimum latency along γ^* so that the uncovered fraction of area is no smaller than p. Then

    ∫_0^|γ^*|f(t)dt = ∫_0^1f_γ^*'(p)dp.

See Figure <ref> for an illustration.





  
Remark We can also use a standard change of variable in Lebesgue integration, however given that the focus of the paper is not on theoretical analysis, we opt for a more elementary and visual approach.

For some 0 ≤ p ≤ 1, suppose j ≥ 2 is such A_j-1 < (1 - p)|R| ≤ A_j. Consider the subpath of γ^* from the starting point until an area of (1 - p)|R| is covered, the length of which is f'_γ^*(p) by definition. Since doubling that subpath gives a tour that covers an area of (1 - p)|R|, we have 2^j-1≤ 2f'_γ^*(p). 

On the other hand, γ is a concatenation of tours of length no longer than c2^j covering an area of A_j≥ A_j for j = 1, 2, …. Thus, the distance traveled along γ by the robot before covering an area of p is no longer than ∑_k≤ jc2^k ≤ 8c2^j-2. This implies, f_γ(p) ≤ 8cf_γ^*(p) for all (1 - p) > A_1/|R|. Consider that

    ∫_0^1f_γ'(p)dp    = ∫_0^1 - A_1/|R|f_γ'(p)dp + ∫_1 - A_1/|R|^1f_γ'(p)dp.

We have

    ∫_0^1 - A_1/|R|f_γ'(p)dp ≤ 
        8c∫_0^1 - A_1/|R|f_γ^*'(p)dp ≤ 8c∫_0^1f_γ^*'(p)dp.

When the fraction of uncovered area is between 1 and 1 - A_1/|R| (meaning the covered area is between 0 and A_1), f_γ'(p) is no greater than 2c, since following along γ by a distance of 2c covers an area of at least A_1. As a result

    ∫_1 - A_1/|R|^1f_γ'(p)dp ≤∫_1 - A_1/|R|^12cdp = 2cA_1/|R|≤ 2c.

Combining the two inequalities (<ref>) and (<ref>) gives

    E[T|γ] ≤ 8cE[T|γ^*] + 2c.


    Given a c-approximation to the quota lawn mowing problem. Let γ^* be an optimal route for minimizing the expected detection time in a polygonal region R under the lawn mowing search mechanism, in pseudopolynomial time, we can compute a route γ satisfying E[T|γ] ≤ 8cE[T|γ^*] + 2c in pseudopolynomial time.



  
Remark The approximation bound we give includes both a multiplicative and an additive error term. We leave open the problem of removing the additive term. In practice, for a sufficiently large region R which requires a long tour to cover a large area, A_1/|R| should be close to 0.




 §.§ Visibility-based search mechanism

We extend the ideas in the previous subsection to the visibility-based search mechanism, which models a robot entering and navigating through a room with obstacles (e.g., pieces of furniture). The robot is equipped with an omnidirectional camera for sensing, and it detects the stationary target when vision of the target is established.

In a polygonal region R, two points x and y see each other if the line segment xy does not intersect the exterior of R. The visibility region of x, denoted V(x), is the set of all points that x sees. We also define the visibility region of a subset S⊂ R (for example, a route), which we denote by V(S), to be the union of the visibility regions of all points in S (refer to Figure ). Efficient algorithms for computing the visibility region of points and segments in polygonal domains are known <cit.>.



Given a polygonal domain R. We want to compute a route γ⊂ R that minimizes the expected amount of time before the robot sees the target, which we also denote E[T|γ] and define similarly to that of the lawn mowing mechanism. For visibility-based detection, it has been known that minimizing the expected time until detection is NP-hard in a polygon with holes <cit.>. We strengthen the hardness result in the following theorem.


    For visibility-based search, minimizing E[T|γ] is NP-hard, even in a simple polygon.


    We once again show NP-hardness by a reduction from the minimum latency problem, but for a weighted tree, which is known to be NP-hard <cit.>. Given a weighted tree, create a straight-line embedding of the tree using any graph drawing algorithm (see <cit.> for a comprehensive survey about graph drawings) in polynomial time. Then, construct a simple polygon from the embedding, similar to the construction used in the proof of Theorem <ref>. A tour within the polygon minimizing expected detection time yields a minimum latency tour.▪




  §.§.§ The budgeted watchman route problem

The budgeted watchman route problem was first introduced in <cit.>. Given a polygon P and a budget B > 0, compute a route γ such that |γ| ≤ B and the area of the visibility region of γ, |V(γ)| is maximized.

While the budgeted watchman route problem is (weakly) NP-hard, the following approximation algorithm, which we require as a subroutine, was proposed in <cit.>. 



    <cit.> Let P be a simple polygon with n vertices. Given a starting point s, in fully polynomial time (in n and 1/ε), we can compute a route of length no longer than (1 + ε)B that passes through s and sees an area no smaller than any route of length B can see.


Let γ^* be an optimal route for minimizing the expected detection time in a simple polygon P under the visibility-based search mechanism. For B = 2^j where j= 1, …, O(log N) (recall that N is the number of unit-sized integer pixels intersecting P), run the approximation algorithm for the budgeted watchman route problem in Lemma <ref> and concatenate the approximate tours into one, which we denote by γ. Then E[T|γ] ≤ (8 + 8ε)E[T|γ^*] + (2 + 2ε) for any ε > 0.


    The analysis follows through the same way as for the lawn mowing search mechanism. The running time is equal to that of the approximation algorithm to the budgeted watchman problem multiplied by O(log N).▪




§ SIMULATION RESULTS

Algorithms developed to guarantee theoretical bounds in quota TSP-type problems are usually far too complicated and incur impractical running times. Thus, we complement our theoretical analysis with some experimental heuristics that are relatively straightforward to implement. Figure <ref> shows our test polygons with holes in which we want to search for a target with a square scanner and rectilinear movement. 



Our code is written in Python <cit.>, we use the following packages: Shapely <cit.> for processing and handling the geometry of the domain and NetworkX <cit.> for handling the graph-theoretical components of the heuristic. Given below is a high-level description of what we call the exponential tree heuristic.


The main idea is to greedily pick a subset of center points that maximize the total reward, while ensuring the length of the shortest tour through them is no larger than 2^j for j = 0, 1, … The center points should induce a connected graph in 𝒢, and their spanning tree should have cost no greater than 2^j, making the TSP tour on them no longer than 2^j+1. By discarding the center points already visited (by previous tours of smaller maximum sizes) and computing only the TSP tour on the remaining ones, the running time as well as the detection time is improved by a margin.

It is quite infeasible to find an optimal minimum latency tour by brute force, even for small instances, due to the factorial number of possible tours. We thus compare the mean detection time of the route generated by our heuristic with that of a heuristic designed to minimize the total latency tour on all the center points. First, note that it suffices, for purposes of approximating the minimum latency tour within a factor of (1 + ε), to concatenate a number of TSP paths with a geometrically decreasing sequence of number of nodes  <cit.>. Thus, we find the overall TSP tour on N center points, replace the subpath on the first ⌊ε N/1 + ε⌋ center points with the TSP path on those points, then repeat with the next ⌊ε N/(1 + ε)^2⌋, ⌊ε N/(1 + ε)^3⌋, … points. We set ε = 0.01 to get close to the minimum latency tour.

We perform our experiment with 1000 different random generations of the location of the target, the results are given in Table <ref>.



The exponential tree heuristic seems to yield average detection time within a constant factor of that of the minimum latency heuristic, but has faster running time. The search routes generated by the exponential tree heuristic are given in Figures <ref> and <ref>.









  §.§.§ 
 The author thanks his advisor, Joseph S. B. Mitchell for helpful and encouraging discussions on the paper.



  §.§.§ 
 The author has no competing interests to declare that are
relevant to the content of this article.











splncs04
 


 Paper Title: Dynamic Gesture Recognition in Ultra-Range Distance for Effective Human-Robot Interaction.








Dynamic Gesture Recognition in Ultra-Range Distance for Effective Human-Robot Interaction





    1st Eran Bamani Beeri
School of Mechanical Engineering 

Tel-Aviv University

Tel-Aviv, Israel 6997801 

eranbamani@mail.tau.ac.il
2nd Eden Nissinman
School of Mechanical Engineering 

Tel-Aviv University

Tel-Aviv, Israel 6997801 

edennissinman@gmail.com
3rd Avishai Sintov
School of Mechanical Engineering 

Tel-Aviv University

Tel-Aviv, Israel 6997801 

sintov1@tauex.tau.ac.il

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



§ ABSTRACT 
This paper presents a novel approach for ultra-range gesture recognition, addressing Human-Robot Interaction (HRI) challenges over extended distances. By leveraging human gestures in video data, we propose the Temporal-Spatiotemporal Fusion Network (TSFN) model that surpasses the limitations of current methods, enabling robots to understand gestures from long distances. With applications in service robots, search and rescue operations, and drone-based interactions, our approach enhances HRI in expansive environments. Experimental validation demonstrates significant advancements in gesture recognition accuracy, particularly in prolonged gesture sequences.



Human-Robot Collaboration, Human-Robot Interaction, Video Recognition, Ultra-Range Gesture Recognition






§ INTRODUCTION

In recent years, Gesture Recognition (GR) in Human-Robot Interaction (HRI) has been advanced significantly. While research primarily focuses on hand gesture recognition <cit.> using conventional cameras and Kinect devices <cit.>, a critical gap remains in recognizing gestures over distances beyond seven meters <cit.>. Bridging this gap is crucial for seamless collaboration between humans and robots, especially in scenarios like surveillance, drone operations, and search and rescue missions.




Ultra-range gesture recognition (URGR) aims to extend recognition capabilities to 25 meters <cit.>, yet considers only static images as the majority of gesture recognition approaches. However, some gestures are inherently dynamic such as waving and beckoning. Additionally, some dynamic gestures may appear as different if only a single frame is used for inference. For instance, a snapshot of a waving gesture may look like a stop gesture. In addition, URGR often faces challenges like image degradation and noise. Video data offers promise, capturing motion dynamics better <cit.>. Deep learning shows potential for recognizing gestures in videos <cit.>, aiding robots in understanding gestures across distances.

In this paper, we introduce the Temporal-Spatiotemporal Fusion Network (TSFN) for URGR in HRI which is effective within a range of 28 meters and operates in real-time. The proposed TSFN model uses Temporal Convolutional Networks (TCN) <cit.> for temporal dependencies and R(2+1)D 
convolutional networks <cit.> for spatiotemporal feature extraction, advancing ultra-range human-robot interaction.




§ METHODS



 §.§ Problem Formulation and Data Collection


GR across diverse environments and distances is essential for effective human-robot communication. Let V_i ∈ℝ^T × H × W × 3 denote video sequences up to 28 meters, with T, H, and W representing frame quantity, height and width, respectively. GR in a video faces challenges due to temporal dynamics, occlusions, and lighting variations. Given a temporal frame sequence V = {v_1, v_2, ..., v_T} and a set of gestures G, the problem entails finding the function F: V → G mapping video sequences to human gestures.

A diverse dataset 𝒟 = {(X_i, y_i, d_i)}_i=1^N is crucial for accurate gesture recognition. It comprises input video X_i, gesture label y_i, and distance d_i ∈ [4, 28] meters from the camera. Webcam-collected data offers varied scenarios, improving model robustness. Challenges include pose/environmental diversity and image quality degradation with distance, leading to reduced resolution and motion blur. Annotations include ground truth gesture labels and distance, aiding model training/evaluation.




 §.§ Models


Our TSFN model combines TCN and R(2+1)D models to create a robust  GR system from video sequences. TCN captures temporal dependencies and the output of a TCN layer is given by



    Y_TCN = σ(W_TCN∗ V + b_TCN),

where W_TCN and b_TCN are the weights and biases of the TCN layer, the operator ∗ denotes convolution, and σ is the Sigmoid activation function.
Video data necessitates 3D convolutions.
A standard 3D convolution applies a filter W ∈ℝ^K × d × k × k, where K is the number of output channels, d is the temporal depth, and k × k is the spatial dimension, given by:

    Y_t,h,w^k = ∑_c=1^C∑_i=1^d∑_j=1^k∑_m=1^k W_c,i,j,m^k · V_c, t+i, h+j, w+m.

In the TCN model, the 3D convolution is factorized into 1D temporal convolution. R(2+1)D networks extend two-dimensional convolutional operations into the temporal domain, separating spatial and temporal convolutions. The R(2+1)D convolution is expressed as

    Y_R(2+1)D = σ(W_spatial∗ (σ(W_temporal∗ V + b_temporal)) + b_spatial),

where W_spatial and W_temporal are the spatial and temporal convolutional weights, respectively. Finally. the outputs from the TCN and R(2+1)D layers are concatenated to form combined features



    F_combined = [Y_TCN, Y_R(2+1)D].

These features are then passed through three fully connected layers to produce the final gesture classification:



    O = σ(W_fc· F_combined + b_fc),

where W_fc and b_fc are the weights and biases of the fully connected layer. This fusion leverages the strengths of both models to enhance GR accuracy.

To train our TSFN model to address the challenges of recognizing human gestures in a video, particularly when the subject is far from the camera, we propose a novel loss function that takes into account the degradation and other phenomena affecting the results. This novel loss function will be designed to balance classification accuracy with robustness to distance-related degradation and is given by

    ℒ = ℒ_CE + α·ℒ_global + β·ℒ_dist + γ·ℒ_robust
,
where ℒ_CE is the cross-entropy loss and ℒ_global is the global context loss that ensures the global context of the video is captured accurately. Loss
ℒ_dist is the distance aware loss that takes into account the degradation effect due to varying distances given by 



    ℒ_dist = 1/N∑_i=1^N d_i ·ℒ_CE(v_i, y_i).

ℒ_dist penalizes more easily for errors when the subject is farther from the camera. Robustness loss ℒ_robust enhances the model's robustness to various degradations by adding a regularization component that minimizes the variance in predictions due to changes in distance:

    ℒ_robust = 1/N∑_i=1^N∑_j=1^M (ℒ_CE(v_i^(j), y_i) - ℒ̅_CE(y_i))^2.

Component ℒ̅_CE is the mean cross-entropy loss for the true label, α, β, γ are hyper-parameters that control the relative importance of each loss component.




§ EXPERIMENTAL RESULTS


To validate the effectiveness of the proposed TSFN model, we conducted experiments comparing its performance with state-of-the-art video recognition models. Figure <ref> illustrates the six gesture categories we focused on: pointing, thumbs-up, thumbs-down, beckoning, stop, and null gestures. The models were tested on a dataset of human gestures observed from 4 to 28 meters. We collected 15 samples of 4-second videos per meter, totaling 2,250 videos for each gesture, with 150 videos used as the test set. Key performance metrics including accuracy, loss, and Mean Average Precision (mAP) are shown in Table <ref>. Figure <ref> shows the accuracy of the TSFN in GR with respect to different distances. The results clearly show the superiority of TSFN over existing methods.







§ CONCLUSION

TSFN redefines ultra-range gesture recognition, which is crucial for seamless Human-Robot Collaboration. By fusing TCNs and R(2+1)D networks, TSFN excels in understanding gestures over vast distances. Future work includes refining TSFN for real-time applications and exploring its potential in dynamic environments including underwater.

99

ahmedhand
S. Ahmed, K. D. Kallu, S. Ahmed, and S. H. Cho, "Hand gestures recognition using radar sensors for human-computer-interaction: A review," Remote Sensing, vol. 13, no. 3, p. 527, 2021.

biswas2011gesture
K. K. Biswas and S. K. Basu, "Gesture recognition using Microsoft Kinect®," in Proceedings of the 5th International Conference on Automation, Robotics, and Applications, pp. 100–103, IEEE, 2011.


nickel2007visual
K. Nickel and R. Stiefelhagen, "Visual recognition of pointing gestures for human-robot interaction," Image and Vision Computing, vol. 25, no. 12, pp. 1875–1884, 2007.

wachs2011vision
J. P. Wachs, M. Kölsch, H. Stern, and Y. Edan, "Vision-based hand-gesture applications," Communications of the ACM, vol. 54, no. 2, pp. 60–71, 2011.

bamani2024ultra
E. Bamani, E. Nissinman, I. Meir, L. Koenigsberg, and A. Sintov, "Ultra-Range Gesture Recognition using a web-camera in Human–Robot Interaction," Engineering Applications of Artificial Intelligence, vol. 132, p. 108443, 2024.

materzynska2019jester
J. Materzynska, G. Berger, I. Bax, and R. Memisevic, "The jester dataset: A large-scale video dataset of human gestures," in Proceedings of the IEEE/CVF International Conference on Computer Vision Workshops, 2019, pp. 0–0.

lui2012human
Y. M. Lui, "Human gesture recognition on product manifolds," The Journal of Machine Learning Research, vol. 13, no. 1, pp. 3297–3321, 2012.

yang2018temporal
K. Yang, R. Li, P. Qiao, Q. Wang, D. Li, and Y. Dou, "Temporal pyramid relation network for video-based gesture recognition," in 2018 25th IEEE International Conference on Image Processing (ICIP), 2018, pp. 3104–3108.

bu2020human
X. Bu, "Human motion gesture recognition algorithm in video based on convolutional neural features of training images," IEEE Access, vol. 8, pp. 160025–160039, 2020.

tran2018closer
D. Tran et al., "A closer look at spatiotemporal convolutions for action recognition," in Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, 2018.

lea2016temporal
C. Lea et al., "Temporal convolutional networks: A unified approach to action segmentation," in Computer Vision–ECCV 2016 Workshops: Amsterdam, The Netherlands, October 8-10 and 15-16, 2016, Proceedings, Part III, vol. 14, pp. 47-54, Springer International Publishing, 2016.

chen2023cnn
J. Chen, J. Wang, Q. Yuan, and Z. Yang, "CNN-LSTM model for recognizing video-recorded actions performed in a traditional Chinese exercise," IEEE Journal of Translational Engineering in Health and Medicine, 2023.

arnab2021vivit
A. Arnab, M. Dehghani, G. Heigold, C. Sun, M. Lučić, and C. Schmid, "ViViT: A video vision transformer," in Proceedings of the IEEE/CVF International Conference on Computer Vision, pp. 6836–6846, 2021.






 Paper Title: A Reinforcement Learning Based Motion Planner for Quadrotor Autonomous Flight in Dense Environment.



Provably Efficient Adiabatic Learning for Quantum-Classical Dynamics
    Di Luo
    August 5, 2024
====================================================================

empty
empty


§ ABSTRACT 

Quadrotor motion planning is critical for autonomous flight in complex environments, such as rescue operations.

Traditional methods often employ trajectory generation optimization and passive time allocation strategies, which can limit the exploitation of the quadrotor's dynamic capabilities and introduce delays and inaccuracies.

To address these challenges, we propose a novel motion planning framework that integrates visibility path searching and reinforcement learning (RL) motion generation.
Our method constructs collision-free paths using heuristic search and visibility graphs, which are then refined by an RL policy to generate low-level motion commands.

We validate our approach in simulated indoor environments, demonstrating better performance than traditional methods in terms of time span.





§ INTRODUCTION



Quadrotors are extensively used in a variety of applications, including rescue operations, fire and electricity inspection, and package delivery.
Achieving autonomous flight in complex environments necessitates effective motion planning, which is crucial for ensuring safe and efficient navigation.
State-of-the-art motion planning algorithms for quadrotors primarily focus on generating collision-free and dynamic feasible trajectories, subsequently optimizing them to achieve smoothness and aggressiveness.
Time allocation plays a pivotal role in the trajectory generation process, as suboptimal time allocation can lead to inefficient trajectories and underutilization of the quadrotor's dynamic capabilities.


However, traditional methods predominantly employ passive time allocation strategies, which may not fully exploit the quadrotor's dynamic capabilities within its constraints.
Additionally, the trajectories generated by these optimizers must be tracked by a controller, introducing delays, increased computational overhead, and discrepancies between the optimized and actual flight trajectories.


To address these challenges, we propose a novel reinforcement learning (RL)-based motion planner for quadrotor autonomous flight in dense environments. Our approach aims to maximize the utilization of the quadrotor's dynamic capabilities while relying only on a low-level controller.

Our method employs a path searching algorithm based on heuristic search and visibility graphs, which constructs a collision-free, minimum-length path in a discrete grid space.
This initial path is then converted into waypoints, serving as control points for the RL motion generator. 
The policy network generates low-level control commands based on observations of the quadrotor's state, control points, and environmental information, optimizing the quadrotor's motion (Fig. <ref>).

These low-level commands are directly sent to the quadrotor's controller, ensuring motion within predefined limits of velocity, acceleration, and attitude.


Compared to existing motion planners, our method deviates from conventional trajectory generation and optimization frameworks by directly generating motion using an RL policy.
It can handle discrete paths that may not satisfy kinodynamic and non-holonomic constraints, producing aggressive, collision-free motion within the specified dynamic constraints.

We demonstrate the effectiveness and robustness of our method through extensive simulations and discuss its potential transferability to real-world quadrotor platforms, that will do later. 
Our key contributions are summarized as follows:


    
  (1) This letter propose a robust and efficient RL-based method that integrates visibility path searching and RL motion generation, directly generating feasible and aggressive motions from the policy.
    
  (2) We incorporate heuristic search and visibility graph concepts to develop a grid map-based pathfinding algorithm that generates collision-free, minimum-length paths.
    
  (3) We investigate optimal RL environment settings and obstacle information configurations for planning in dense environments, enabling motion generation from any initial state.
    
  (4) We provide extensive simulation evaluations of the proposed method and discuss its potential transferability to real-world quadrotor platforms.






§ RELATED WORK




 §.§ Optimal Control Methods

Quadrotor motion planning has been extensively studied, with optimal control-based trajectory optimization methods prevailing as the predominant approach.

These methods focus on generating smooth, dynamically feasible, and collision-free trajectories by solving optimization tasks modeled as optimal control problems.

The pioneering approach is the minimum-snap trajectory generation<cit.> in differential flatness outputs, representing the trajectory as a polynomial and using gradient descent to iteratively modify the time segments.
Several works propose two-stage solving methods where trajectory generation occurs in the second stage <cit.>.
For instance, <cit.> introduces a safe flight corridor composed of convex polyhedra, providing constraints for optimization.
The method in <cit.> combines sampling-based and gradient optimization methods to achieve smooth, feasible, and safe trajectories.
<cit.> utilizes B-spline for kinodynamic path searching, adopting an elastic optimization method to refine trajectories.
The Euclidean Signed Distance Field (ESDF) map is employed in <cit.> to obtain collision potentials and in <cit.> to find a path in the velocity field, improving time allocation.
<cit.> proposes a comprehensive pipeline for quadrotor motion planning, including kinodynamic path searching, B-spline trajectory generation, time allocation, and nonuniform B-spline optimization based on ESDF.
Subsequent works <cit.> introduce topological path and risk-aware yaw planning.
Given the computational intensity of constructing ESDF, <cit.> proposes an ESDF-free gradient-based planner.
However, <cit.> demonstrates a method to generate ESDF rapidly.
These methods effectively generate smooth, safe, and feasible trajectories. However, they may be conservative regarding the quadrotor's dynamic capabilities due to passive time allocation. In contrast, <cit.> and <cit.> focuses on spatial-temporal deformation, which aligns with our approach by considering both geometric and temporal planning concurrently.



 §.§ RL based Methods

RL-based methods have gained significant traction in quadrotor motion planning due to their capability to optimize non-convex and discontinuous objectives, which traditional approaches often struggle to handle effectively.
In <cit.>, the authors propose an imitation learning (IL)-based teacher-student pipeline that directly maps noisy sensory depth images to polynomial trajectories.
While IL is quite efficient, its scalability is limited by the need for expert data collection.
<cit.> employs a topological path searching algorithm to generate guiding paths, followed by RL to generate control commands.

This two-stage method performs well in selected scenarios, outperforming some traditional methods.
<cit.> extracts features from depth images and uses a teacher-student IL framework similar to <cit.>. 
Furthermore, it generates control commands directly and includes a perception reward to guide yaw planning.
In <cit.>, Song investigates the strengths and limitations of optimal control and RL, proposing that RL is advantageous as it can optimize better objectives that may be infeasible for optimal control methods to address.


Current RL-based methods often incorporate high-order dynamic parameters like body rate and attitude into their observations to compose trajectories or low-level control commands. Despite their advanced techniques, these methods frequently rely on imitation learning (IL) with expert knowledge or are tested in fixed environments, limiting their generalization and transferability to unknown environments, and failing to enable autonomous flight in new scenarios.
Our RL approach also uses high-order dynamic parameters as observations but focuses on simplifying the learning tasks for RL. 
Instead of expecting RL to handle the entire path searching process, we use RL as an optimizer to refine motion and control points, similar to B-spline optimization schemes. 
This strategy allows for more efficient learning and better adaptation to complex environments.



§ SYSTEM OVERVIEW






 §.§ Quadrotor dynamics

We model the quadrotor as a linear system with translational and rotational dynamics decoupled.
The dynamics equations are


    p̈_e = -g e_3 + ∑_i=0^3f_i/mR_b^e b_3 - k_d v_e^3/||v_e||



    Jω̇^b=-ω^b ×Jω^b + G_a+τ


where p_e is position in earth frame, v_e is velocity, R_b^e is rotation matrix from body frame to earth frame, b_3 is z axis in body frame, f_i is thrust of each rotor, m is mass, k_d is drag coefficient, J is diagonal inertia matrix, ω^b is angular velocity in body frame, G_a and τ=[τ_x τ_y τ_z]^T is gyroscopic torque and torque by thrusts respectively.
We use Euler angles to represent rotation and set a limit on roll and pitch angles to avoid singularity.

The control allocation matrix that can be used to get relation between thrusts and torques is given by


    [       1       1       1       1; -l/√(2) -l/√(2)  l/√(2)  l/√(2); -l/√(2)  l/√(2)  l/√(2) -l/√(2);      -κ       κ      -κ       κ ][ f_0; f_1; f_2; f_3 ]
        =
        [ f_T; τ_x; τ_y; τ_z ]


where κ denotes rotor's torque coefficient, and l represents the distance from the center of mass to the rotor center.
The complete state of the quadrotor is defined as 𝐱=[p_e, ṗ_̇ė, p̈_̈ë, Θ, ω_b] , with Θ=[ ϕ, θ, ψ]^T indicating roll, pitch and yaw angle.The control input is given by 𝐮 = [f_0, f_1, f_2, f_3]^T. 
We use simpson's rule to do numerical integration for state update.



 §.§ Visibility RL Planning Structure


The proposed planning framework is illustrated in Fig. <ref>.
The planning process operates in two stages. 
First, the Visibility Path Searching algorithm constructs a reference path from the start to the goal, ensuring obstacle avoidance and path length minimization. 
This reference path is discretized into waypoints based on the current position and the distance to the next obstacle corner (Section <ref>).
Second, the RL Motion Generation method produces control commands to optimize the quadrotor's motion. These commands enable the quadrotor to follow the waypoints within a certain tolerance, determining the required proximity to waypoints and strategies for obstacle avoidance in continuous space, as presented in Section <ref>.





§ VISIBILITY PATH SEARCHING



Our pathfinding algorithm integrates the A*, Jump Point Search (JPS)<cit.>, and Visibility Graph methods.

We inherit the heuristic function and the open and closed list architecture from the A* algorithm but use a different cost function. 
Elements of the JPS method are incorporated to reduce the number of nodes in the open list, minimizing computational costs associated with sorting these nodes. 
Additionally, we leverage the optimal distance characteristics of the Visibility Graph method without constructing complex polygon edges and vertices.


The proposed strategy facilitates omnidirectional movement through obstacle corner detection and visibility checks, rather than restricting movement to increments of π/4 in 2D plane.
This approach is implemented by selecting obstacle corners in the Corner() function and assessing voxel visibility in the VisibleCheck() function. Consequently, this method reduces the computational and memory overhead typically required for accessing each vertex and edge of the voxels. 
The detailed algorithm is presented in Algorithm <ref>.





 §.§ Obstacle Corner Search

We define an obstacle corner as a voxel with only one occupied neighbor, indicating its location at the edge of an obstacle.

In the context of the Visibility Graph, the optimal path from the start to the goal point traverses vertices that represent the corners of obstacles.
Therefore, obstacle corners Corner(), including the start and goal points, are exclusively utilized as elements in the open list.
This approach reduces the number of nodes in the open list and decreases sorting costs, similar to JPS, while maintaining the optimal path characteristics of the Visibility Graph.

The cost of a obstacle corner using EuclideanCost() is defined as


    g(n) = g(n-1) + ||p_n - p_n-1||




 §.§ Visibility Expansion

When a new obstacle corner is detected, the Bresenham algorithm<cit.> is employed to perform a visibility check using VisibleCheck(), determining whether the corner is visible from the current node. 
Only corners confirmed to be visible are considered as child nodes and candidates for the open list, ensuring that a connected path of vertices via line segments is traced from start to goal.

We adopt an expansion method from Hybrid A*<cit.> in RandomVisibleCheck() to verify the existence of a collision-free path between the current node and the goal along a line segment. 
The invocation frequency of this function increases as the distance to the goal decreases.




§ RL MOTION GENERATION



 §.§ Policy Framework

We construct the control points optimizing and motion generating problem in a reinforcement learning (RL) framework using Markov Decision Process (MDP) defined by tuple (𝒮, 𝒜, 𝒫, ℛ).
The quadrotor agent begins in a state s_t ∈𝒮 that is selected from state space distribution ρ(𝒮).


In this setting, the agent interacts with the environment by sampled action a_t ∈𝒜 from a random policy π(a_t|s_t).
After that, the state of the agent will be updated from s_t to s_t+1 by the transition probability 𝒫(s_t+1|s_t, a_t), and a reward ℛ(s_t, a_t, s_t+1) will be given to the agent.
Detailed configuration of the state space, action space, and reward function will be discussed in the following sections.




  §.§.§ Observation and action space


The observation space contains three main parts: state of the quadrotor, waypoints information and ESDF based obstacle details.
Specifically, we denote the state vector as


    s = [ p_e(t), v_e(t), ω_b(t), Θ(t), p_w_0∼ w_n,  sdf]



And the observation vector is derived from the state vector, incorporating some elements directly from it, including v_e(t), ω_b(t), Θ(t), sdf.
We convert other state elements to low level features, which would be more generalized and easier for policy training.
For example, waypoints position combined with absolute position of the quadrotor are used to calculate relative position of the quadrotor to target waypoints.
We also add a normalized vector representing the direction from current position to the next waypoint in body frame, which is used to guide the limit of field of view (FOV) of depth camera with direction of [1,0,0]^T in body frame.
The detailed obstacle information is composed by the ESDF map of the environment, which will be discussed in the following section.


The action produced by RL policy is a(t)=[ f_T, ω_b^req], f_T is the collective thrust by all motors and ω_b^req is the desired body rate, which has been proved to perform well in learning-based quadrotor control and planning tasks.
Although this control command is usually used in drone racing tasks due to its low latency and high frequency, it also takes a significant gap between simulation and real world.
Despite this drawback, this approach can result in better smoothness in acceleration and velocity, which will be beneficial in energy saving and safety.





  §.§.§ Reward function

According to paper RL vs OC, RL outperforms Optimal Control (OC) in terms of optimizing a better objective that OC may not be able to solve.
From this perspective, we design a reward function that explicitly represents better active time allocation, collision avoidance, and smoothness in motion based on RL vs OC.
The reward function is defined as


    r(t) = k_p r_p(t) + k_s r_s(t) + r_a + r_c + k_r ||ω_b||


where r_p(t) is the customized progress reward, r_s(t) is punishment for state violation, r_a is the reward for arriving at one waypoint, r_c is the punishment for collision, and k_r ||ω_b|| is the penalty for large angular velocity.

Specifically, the progress reward r_p(t) is defined as


    r_p(t) = ||p_wpt-p_e(t-Δ t)||-||p_wpt-p_e(t)||/v_maxΔ t - r_t


We normalize the progress reward within interval [0, 1] and subsequently introduce a negative term r_t, thereby imposing a time penalty which facilities time allocation and maximized the utilization of the quadrotor's dynamic capabilities.

For state violation penalty r_s(t), we impose limits on acceleration and velocity to ensure dynamic smoothness and safety.
Additionally, attitude limits are set to prevent overly aggressive maneuvers and avoid singularities.



    r_s(t)=
        
            0,    if all constraints are satisfied
    
            -n,     n∈ [0,4]  otherwise


Specifically, n represents the number of elements from the set [ v_e, a_e, ϕ, θ] that exceed their prescribed limits.





 §.§ Training methodology


We create a training environment based on OpenAI gym, providing an interface for creating a customized environment.


At the beginning of each episode, the velocity, body rate, orientation, waypoints and obstacles are randomly initialized following a predefined probabilistic pattern which is shown in Fig.<ref>, while the position if fixed at the center of the map.



  §.§.§ Quadrotor state initialization

Since we are using only two waypoints for the whole navigation, we make the first waypoint totally randomized with only the distance from quadrotor position to the waypoint withing a certain range.


    p_w_0 = p_e + u_0 · d_p0,    d_p0∈ [d_min, d_max]


where u_0 is a randomly generated unit vector, and d_min and d_max define the waypoint distance range. The second waypoint is given by

The second waypoint is set to within a certain distance and angle from the first waypoint and the line from quadrotor position to the first waypoint.


    p_w_1 = p_w_0 + u_1 · d_01


where u_1 is a unit vector such that θ_01≤θ_wpt, with θ_01 being the angle between u_0 and u_1.
d_01 must lie within the waypoint distance range and ensure that the distance between the quadrotor and the second waypoint exceeds d_min.



Concerning velocity, constraints are applied to ensure the velocity direction does not deviate significantly, expressed as θ_v0≤θ_vel, as the path searching algorithm is expected not to produce waypoints that are in a direction completely opposite to the current velocity.
We also set constraints on the orientation of the quadrotor as


    ϕ, θ∈[-θ_xy, θ_xy],   ψ∈[-θ_z+ψ_0, θ_z + ψ_0]




  §.§.§ Obstacles setting


In our framework, RL is expected to generate motions based on collision-free waypoints obtained after path searching. 
These waypoints might still be close to or intersect with obstacles after sampling. 
Therefore, we only need to place obstacles on or near the lines from the quadrotor to the waypoints, rather than creating a maze-like environment. 
Additionally, it is essential to ensure a feasible path exists between the quadrotor position and the waypoints (see Fig. <ref>).






  §.§.§ Obstacle avoidance


Although the guided waypoints from path searching can avoid obstacles in voxel level, the quadrotor still needs to avoid obstacles in continuous space, which is achieved adding related observation and reward in RL framework.


We truncate ESDF map into TSDF map due to two reasons: first, we expect RL framework as a local motion generator to be more efficient in terms of active time allocation and collision avoidance; second, the ideal range of depth camera is limited, which is more suitable for TSDF map.




While SDF maps can address some motion planning problems, they often require the entire map or significant portions. 
In an RL framework, excessive information can hinder generalization. Using SDF data can aid in obstacle avoidance but is not comprehensive for all scenarios, as illustrated in Fig. <ref>. 
A more efficient method for quadrotor environmental understanding is necessary.




To address this issue, we sample ray castings from the quadrotor's position to the first waypoint at irregular angles, centered around the waypoint direction (see Fig. <ref>).
The end positions of each beam are used in the observation space. 
Although this method enhances environmental understanding, it struggles with generalization due to two main reasons: it requires numerous rays for comprehensive environment understanding, increasing observation space complexity, and it varies significantly across different obstacle settings, hindering the training of a policy that generalizes well in diverse scenarios.




To address these issues and better align with our objectives, we propose a method based on the safe corridor and visibility graph concepts named ray search.
The core idea is to define safe regions that guide the quadrotor's velocity towards the waypoint, enhancing both efficiency and reliability (see in Fig. <ref>).

We first cast rays from the quadrotor towards the waypoints, spreading them in a cone shape to detect obstacles' edges and free space within a certain range. 
If the direct ray to the waypoint is blocked, we search for a transition point from obstacle to free space. 
If unblocked, we identify the obstacle edges. 
The final ray search observation for a waypoint is given as  ray = [ p_wi ∼e1 - p_e, p_wi ∼e2 - p_e, d_fi].













 


§ RESULTS




 §.§ Training details

The policy is trained using the Proximal Policy Optimization (PPO)<cit.> algorithm in Stable-Baselines3<cit.>, which has demonstrated strong performance in various continuous control tasks, including drone racing. 
We train two separate models for scenarios with and without obstacles to streamline training and improve efficiency. 
This approach also mitigates the issue of forgetting, which we encountered when sequentially training a single model first on obstacle-free and then on obstacle-present cases.
The decision to use a specific model depends on the values from the ESDF map and the camera's ideal range.



The quadrotor's physical properties and the RL algorithm's hyperparameters are listed in Table <ref>. 
We utilize the Iris quadrotor model from the PX4 firmware, adjusting its mass to 0.535 kg and the torque constant to 0.12 for our purposes.
Following previous research, we employ Runge-Kutta 4th order numerical integration to update the quadrotor's state, excluding aerodynamic drag during training due to its insignificance for the current v_max.

All policies are trained on a desktop computer equipped with an NVIDIA RTX 4090 GPU and an Intel i9-13900K CPU.



 §.§ Analysis and Comparison

We compare our method with Fast-Planner <cit.> in terms of time to reach the same waypoint, energy consumption (integral of jerk), and computation time, with plans to include additional baselines in future work. 
Fast-Planner and our method employ ESDF for path searching and motion or trajectory generation. 
To ensure fairness, we remove control disturbances and initialize both methods from the same state.
Our evaluation is conducted in two densely obstructed indoor scenarios, using identical models to assess and compare performance against Fast-Planner.
The maximum velocity and acceleration for baseline methods are set to be 3m/s and 6m/s^2, respectively.
For collision detection in the configuration space, the baseline methods use the same inflated obstacle size as ours, 0.3m.
Additionally, we impose stricter limits on z-axis position (1 ± 0.3m), sensing horizon (5.0m), and searching horizon (7.5m) to align with our method.
Other parameters are set to default values.







The comparison of Fast-Planner and our method is shown in Fig. <ref> and Table <ref>.
To date, our method has demonstrated better performance in terms of time span, although the energy consumption remains much higher than that of Fast-Planner due to the aggressive and oscillatory control commands generated by the RL policy. 
Additionally, our method is less risk-aware compared to Fast-Planner, as it does not incorporate active perception, highlighting a potential direction for future work.



§ CONCLUSION


In this letter, we propose a novel quadrotor motion planning framework that integrates path searching and motion generation using visibility-based methods and reinforcement learning.
Our approach employs visibility path searching to generate safe waypoints with minimal path length, which are further optimized by an RL policy to produce low-level motion commands.
We validate our method in two simulated indoor environments and compare it with Fast-Planner, demonstrating better performance in terms of time span.

Future work will focus on optimizing the RL policy to reduce energy consumption and enhance risk awareness through active yaw planning. 
We also plan to extend our method to real-world scenarios and evaluate its performance on a real quadrotor platform soon.
Additionally, we aim to explore the potential of our approach in dynamic environments.



§ ACKNOWLEDGMENT


The authors would like to thank Yunlong Song for RL and sim to real transfer suggestions, Zeshuai Chen and Jia Xu for control and real-world platform discussions, Tao Cui and Yubo Dong for RL environment setup discussions.


IEEEtran


-12cm   
                                  
                                  
                                  
                                  
                                  




 Paper Title: MuJoCo MPC for Humanoid Control: Evaluation on HumanoidBench.




Anti-correlation between Flux and Photon Index of Hard X-ray Emission from The Crab
    [
    August 5, 2024
===================================================================================

empty
empty

§ ABSTRACT 
We tackle the recently introduced benchmark for whole-body humanoid control  <cit.> using MuJoCo MPC <cit.>.
We find that sparse reward functions of  yield undesirable and unrealistic behaviors when optimized; therefore, we propose a set of regularization terms that stabilize the robot behavior across tasks.
Current evaluations on a subset of tasks demonstrate that our proposed reward function allows achieving the highest  scores while maintaining realistic posture and smooth control signals.
Our code is publicly available and will become a part of MuJoCo MPC[<https://github.com/google-deepmind/mujoco_mpc/pull/328>],
enabling rapid prototyping of robot behaviors.






§ INTRODUCTION AND RELATED WORK

Designing dynamic behaviors for humanoid robots is a challenging problem <cit.>.
One promising approach is to craft task-specific reward functions and train policies via Reinforcement Learning (RL).
To this end, <cit.> proposed , a suite of 27 tasks for Unitree H1 robot based on MuJoCo <cit.>, with 12 tasks assessing locomotion abilities and 15 tasks evaluating object manipulation skills.
In <cit.>, performances of four popular RL algorithms are reported: DreamerV3 <cit.>, TD-MPC2 <cit.>, SAC <cit.>,  PPO <cit.>.
However, none of these methods is able to solve the tasks satisfactorily, producing
jittery motions ill-suited for execution on real hardware.
Therefore, we evaluate an alternative approach to humanoid behavior synthesis that exploits the power of simulation: Model Predictive Control (MPC).

MPC does not require training; at each time-step, it selects the optimal action based on a multi-step lookahead search, and repeats this procedure at the next time-step in a receding-horizon manner <cit.>.
MPC is a mature paradigm that admits various planning algorithms for this search, including iLQG <cit.> and even randomly sampled action sequences <cit.>.
We apply MPC to a subset of the  locomotion and manipulation tasks using MuJoCo MPC (MJPC) <cit.>, which implements the aforementioned planners.




§ MJPC WITH SHAPED REWARDS




MPC repeatedly solves a finite-horizon optimal control problem min_u_0:T∑_t=0^Tc(x_t, u_t) where c(x_t, u_t) is the instantaneous cost.
MJPC further decomposes the cost into a weighted sum 
c(x,u) = ∑_i=0^M w_i · n_i (c_i(x,u))
where c_i(x,u) are (signed) residuals, n_i are twice-differentiable norm functions, and w_i are tunable non-negative weights <cit.>.

The  tasks are formulated in terms of rewards r_hb rather than costs.
Therefore, we apply a transformation c_hb(x,u) = |r_max - r_hb| before passing it to MJPC, where r_max is the maximum achievable instantaneous reward (set to r_max = 1) and the norm is approximated by  to ensure differentiability. 

We observed that r_hb alone yields physically undesirable behaviors (Fig. <ref>) and results in low rewards (Fig. <ref>).
Therefore, we introduced a number of shaping terms that i) improve robot stability and ii) provide dense reward signal.

Stability-enhancing cost terms penalize excessively strong movements and promote postural balance:
1) height of the robot's head from the ground;
2) height difference between the pelvis and the feet;
3) linear velocity of the center of mass (CoM);
4) `balance' (projection of CoM between the feet);
5) deviation from a `canonical' posture;
6) direction the robot is facing;
7) magnitude of the control signal.


Dense reward residuals:
1) distance between an object and its target location;
2) distance between the left hand and the object;
3) distance between the right hand and the object.
Although some of these residuals are already encoded in the  reward, we find that providing them individually to the optimizer improves  performance (Fig. <ref>).



§ EXTENDED TASK EVALUATION PROTOCOL


Along with the pointed out uninformativeness of the reward function in , we observe one further factor that impacts the generated behaviors and evaluations—namely, the episode length.
The evaluation period
is very short—2 seconds for the  and  tasks; and for the  task, the simulation terminates as soon as the box reaches its target location.
Such short episode lengths result in policies that do not maintain balance at the end of the episode, not expecting further tasks (Fig. <ref>, bottom).
We provide a quantitative evaluation of the impact of the episode length in Fig. <ref> on the  task.
A qualitative evaluation is provided in Fig. <ref>, where new targets are re-spawn, requiring the robot to perform the task repeatedly.
Based on these evaluations, we argue for longer episode lengths and for repeated tasks with changing goals. 




§ ANALYSIS OF BEHAVIORS AND PLANNERS

In this section, we analyze the generated behaviors in a quantitative manner, beyond only reporting the return value.
r0.6
    

    Control smoothness cost: average squared joint velocity.
    
    Task     MPC-ours     MPC-hb 

         1.45 ± 0.13     1.67 ± 0.87 

         0.01 ± 0.00     1.49 ± 0.23 

         1.60 ± 0.14     1.66 ± 0.05 

    
    
    


First, we evaluate the smoothness of trajectories (energy cost), as judged by the average squared joint velocity (Table <ref>).
Our reward generates consistently smoother trajectories on the  and  tasks (while also achieving much higher reward—see Fig. <ref>).
On the  task, the cost is comparable due to the short episode duration.
Analysis with respect to other criteria will be provided in an extended version of the article.


While cheap at train-time (no pre-training needed), MPC is expensive at test-time because of online re-planning.
Therefore, the choice of the i) planner, ii) planning horizon, and iii) number of planning iterations is crucial.
Table <ref> shows the run times of our experiments on a MacBook Air M1, 16GB, averaged over 12 runs.
Each planner was given a fixed number of iterations to ensure a consistent comparison (normally, the planner would run as many iterations as possible within a real-time simulation timestep).




§ CONCLUSION


In this paper, we made a step towards enabling easy experimentation with MPC for humanoid robot control in simulation.
We ported  to MuJoCo MPC and provided evaluations on a subset of tasks.
We showed that the current reward functions are insufficient and proposed shaping terms.
Furthermore, we identified an issue with short episode lengths and argued for evaluating on repeated tasks with changing goals.
Our results show superior performance of our method over MPC and RL baselines.

plain
 



 Paper Title: U2UData: A Large-scale Cooperative Perception Dataset for Swarm UAVs Autonomous Flight.


	
	
	
	
	0000-0003-4734-5607
	
		Department of Computer Science and Technology, Tsinghua University
		Beijing
		China
	
	fengtongtong@tsinghua.edu.cn
	
	
	Corresponding Authors. BNRist is the abbreviation of Beijing National Research Center for Information Science and Technology.
	
		Department of Computer Science and Technology, BNRist, Tsinghua University, Beijing, China
		
	
	xin_wang@tsinghua.edu.cn
	
	
	
		Department of Film and Television Technology, Beijing Film Academy
		Beijing
		China
	
	hanfeilin@bfa.edu.cn
	
	
	1234-5678-9012
	
		Department of Film and Television Technology, Beijing Film Academy
		Beijing
		China
	
	zhangleping@mail.bfa.edu.cn
	
	
	[1]
	
		Department of Computer Science and Technology, BNRist, Tsinghua University, Beijing, China
		
	
	wwzhu@tsinghua.edu.cn
	
	
	
	§ ABSTRACT 
		Modern perception systems for autonomous flight are sensitive to occlusion and have limited long-range capability, which is a key bottleneck in improving low-altitude economic task performance. Recent research has shown that the UAV-to-UAV (U2U) cooperative perception system has great potential to revolutionize the autonomous flight industry. However, the lack of a large-scale dataset is hindering progress in this area. This paper presents U2UData, the first large-scale cooperative perception dataset for swarm UAVs autonomous flight. The dataset was collected by three UAVs flying autonomously in the U2USim, covering a 9 km^2 flight area. It comprises 315K LiDAR frames, 945K RGB and depth frames, and 2.41M annotated 3D bounding boxes for 3 classes. It also includes brightness, temperature, humidity, smoke, and airflow values covering all flight routes. U2USim is the first real-world mapping swarm UAVs simulation environment. It takes Yunnan Province as the prototype and includes 4 terrains, 7 weather conditions, and 8 sensor types. U2UData introduces two perception tasks: cooperative 3D object detection and cooperative 3D object tracking. This paper provides comprehensive benchmarks of recent cooperative perception algorithms on these tasks.
	
	
	
		<ccs2012>
		<concept>
		<concept_id>10010147.10010178.10010187.10010194</concept_id>
		<concept_desc>Computing methodologies Cognitive robotics</concept_desc>
		<concept_significance>500</concept_significance>
		</concept>
		<concept>
		<concept_id>10003120.10003121.10003122</concept_id>
		<concept_desc>Human-centered computing HCI design and evaluation methods</concept_desc>
		<concept_significance>300</concept_significance>
		</concept>
		<concept>
		<concept_id>10010147.10010178.10010219.10010223</concept_id>
		<concept_desc>Computing methodologies Cooperation and coordination</concept_desc>
		<concept_significance>500</concept_significance>
		</concept>
		</ccs2012>
	
	
	[500]Computing methodologies Cognitive robotics
	[300]Computing methodologies Cooperation and coordination
	[100]Computing methodologies Robotic planning
	
	
	
	
	
		
    < g r a p h i c s >

		U2UData is collected by performing swarm UAVs autonomous flight tasks in the U2USim environment. Top left: Swarm UAVs autonomous flight task, where each UAV protects an animal based on the arrow. Top right: First-person views and LiDAR images of each UAV. Bottom left: U2USim, a real-world mapping swarm UAVs simulation environment. Bottom right: Swarm UAVs cooperative perception benchmark.
		
	
	
	U2UData: A Large-scale Cooperative Perception Dataset for Swarm UAVs Autonomous Flight
    Wenwu Zhu
    
======================================================================================



§ INTRODUCTION

	
	
	
	Perception is critical in the autonomous flight task of Unmanned Aerial Vehicles (UAVs) for accurate navigation and safe planning<cit.>. The recent development of deep learning brings significant breakthroughs in various perception tasks such as 3D object detec- tion<cit.>, object tracking<cit.>, and semantic segmentation<cit.>. However, single-UAV vision systems still suffer from many real-world challenges, such as occlusion and short-range perception capability<cit.>, which can cause catastrophic accidents and are key bottlenecks in improving low-altitude economic task performance. The shortcomings are mainly due to the limited field-of-view of individual UAVs, resulting in an incomplete understanding of the environment.
	
	A growing interest and recent advances in cooperative perception systems<cit.> have enabled a new paradigm that can potentially overcome the limitations of single-UAV perception. By leveraging UAV-to-UAV (U2U) technologies, multiple connected and automated UAVs (UAVs) can communicate and share captured sensor information simultaneously. As shown in Figure <ref>,  swarm UAVs flying autonomously in a dynamic open environment, the ego UAV (blue) struggles to perceive the tracking object (deer) due to leaf obstruction and snow interference. Incorporating the multimodal features of the nearby UAVs (yellow or red) can further distribute multiple tasks and achieve greater flexibility, robustness, and perceptual range, leading to significant advantages in harsh and complex environments.
	
	However, despite the great promise, cooperative perception is mainly focused on the vehicles and ignores the UAV literature, which remains challenging to validate U2U perception in real-world scenarios due to the lack of public datasets. Existing U2U cooperative perception datasets, as shown in Table <ref>, CoPerception-UAVs<cit.> and CoPerception-UAVs+<cit.> rely on open-source simulators such as AirSim<cit.> and CARLA<cit.> and consider only 1 terrain, 1 weather, and 1 to 2 sensor types; they collect datasets using fixed altitude and consistent or fixed formation mode. In real-world scenarios, compared to autonomous driving, autonomous flight has more freedom, faces more complex environments, and is more susceptible to the influence of temperature, humidity, and airflow due to its smaller size. Obviously, there will be a clear domain gap between existing synthetic data and real-world data. As a result, models trained on these datasets may not generalize well to realistic flight situations.
	
	To further advance innovative research on U2U cooperative perception, 1) we present a large-scale cooperative perception dataset (U2UData) for swarm UAVs autonomous flight. It is collected by three UAVs flying autonomously in the U2USim, covering a 9 km^2 flight area, comprising 315K LiDAR frames, 945K RGB and depth frames, 2.41M annotated 3D bounding boxes for 3 classes, and including brightness, temperature, humidity, smoke, airflow values covering all flight paths. Compared with fixed altitude and consistent or fixed formation mode flying, autonomous flight can more comprehensively explore harsh and complex environments, allowing perception models to achieve higher flexibility and stronger robustness. 2) Brightness, temperature, humidity, smoke, and airflow modalities can greatly affect UAV flight control. However, they are closely related to terrain and meteorology, and have strong interactions with each other, making them difficult to collect. We build the first real-world mapping swarm UAVs simulation environment, U2USim, including 4 terrains, 7 weather conditions, and 8 sensor types. It takes Yunnan Province as the prototype and uses the real meteorological data of Yunnan Province collected by the China Meteorological Center[https://data.cma.cn/] to map the simulation environment based on longitude and latitude. It enables UAVs to perceive the world like humans and make optimal decisions. 3) U2UData introduces two perception tasks: cooperative 3D object detection and cooperative 3D object tracking. 4) We have provided 8 state-of-the-art cooperative perception algorithms for benchmarking, and will make all the data, benchmarks, and models publicly available worldwide.
	
	Our contributions can be summarized as follows:
	
		
  * Dataset. We present U2UData, the first large-scale cooperative perception dataset for swarm UAVs autonomous flight.
		
  * Simulation. We build U2USim, the first real-world mapping swarm UAVs simulation environment, taking Yunnan Province as the prototype, including 4 terrains, 7 weather conditions, and 8 sensor types.
		
  * Benchmark. We introduce two cooperative perception tasks, including cooperative 3D object detection and cooperative 3D object tracking, and provide comprehensive benchmarks with 8 SOTA models. The results show the effectiveness of U2UData in multiple tasks.
	
	
	

§ RELATED WORK

	
	
	
	Cooperative perception. Due to the inherent limitations of an agent's own sensors (e.g., camera/LiDAR), occlusions, sensor degradation or failure, and long-range perception are extremely challenging for single-agent systems, with potentially disastrous consequences in harsh and complex environments. Cooperative perception systems can learn how to share information among multiple agents to perceive the environment better than individually.  Existing cooperative perception approaches can be roughly divided into three categories: (1) Early Fusion<cit.>. Agents transmit raw sensor data directly to other collaborators, and the ego agent makes task decisions based on the fused raw data, which preserves complete information but requires a large bandwidth. (2) Late Fusion<cit.>. Each agent makes task decisions using its sensor data and delivers the decision results to others. The ego agent applies Non-maximum suppression to produce the final outputs, which maintain small transmission bandwidths but cannot achieve deep fusion of sensor information. (3) Intermediate fusion<cit.>. Adjacent UAVs utilize a neural feature extractor to derive intermediate features, which are then compressed and sent to the ego UAV for cooperative feature fusion. This method maximizes the benefits of both early and late fusion and is well-suited for extensive deployment scenarios. However, despite its great promise, cooperative perception mainly focuses on vehicles and ignores the UAV literature, which remains challenging to validate U2U perception in real-world scenarios due to the lack of public datasets.
	
	Cooperative perception datasets. Collecting datasets<cit.> is crucial for advancing algorithmic research. Public cooperative perception datasets have significantly accelerated progress in autonomous driving technologies in recent years. For instance, OPV2V<cit.> introduced the first 3D cooperative perception dataset, leveraging CARLA<cit.> and OpenCDA<cit.> joint simulation environment. DAIR-V2X pioneered real-world datasets for cooperative detection. V2x-seq<cit.> developed the initial sequential dataset for vehicle-infrastructure cooperative perception and forecasting. V2X-Sim<cit.> further explored vehicle-to-everything perception feasibility using synthesized data from the CARLA simulator<cit.>. V2V4Real<cit.> established the first large-scale real-world dataset for vehicle-to-everything cooperative perception. However, cooperative perception datasets in autonomous flight scenarios remain scarce. Existing U2U cooperative perception datasets, such as CoPerception-UAVs<cit.> and CoPerception-UAVs+<cit.>, rely on open-source simulators like AirSim<cit.> and CARLA<cit.>, featuring limited terrain, weather, and sensor types. These datasets collect data at fixed altitudes and in consistent or fixed formation modes. In contrast to autonomous driving, UAVs' autonomous flight presents greater freedom, encounters more complex environments, and is more influenced by natural weather due to their smaller size. Hence, there exists a notable domain gap between existing synthetic data and real-world data, potentially limiting the generalization of models trained on these datasets to realistic flying scenarios. In this paper, we introduce the first large-scale cooperative perception dataset for swarm UAVs autonomous flight.
	
	
	
	

§ U2USIM ENVIRONMENT

	A simulator for swarm-UAVs must be able to more realistically simulate dynamic physical characteristics<cit.> (such as collision); sensors such as IMU<cit.>, camera<cit.>, GPS<cit.>, lidar<cit.> etc; and interaction with the ROS ecosystem<cit.>. In real-world scenarios, UAVs are extremely sensitive to the effects of temperature, humidity, and airflow due to their small size. Existing UAV simulators, as shown in Table <ref>, can visually realize digital twins<cit.> of the real world through GPU rendering, but it is difficult to provide modal information other than visual and LiDAR modalities. Many existing studies manually set values based on experience for temperature, humidity, brightness, smoke, and airflow modalities. However, because sensors are closely related to terrain and meteorology, and have strong interactions between modalities, the model trained by existing simulators is difficult to deploy in the real world. 
	
	In this section, we build U2USim, the first real-world mapping swarm-UAVs simulation environment, taking Yunnan Province as the prototype, including 4 terrains, 7 weather conditions, and 8 sensor types. We use actual meteorological data from Yunnan Province collected by the China Meteorological Center to map the simulation environment based on longitude and latitude. U2USim can not only significantly reduce the domain gap, but UAVs can also use it to perceive the world and make optimal decisions like humans.
	
	

 §.§ U2USim Design

	U2USim map. Yunnan Province is located in southwest China, with high terrain in the northwest and low terrain in the southeast; the elevation range is [76.4, 6740]m. Yunnan Province has a subtropical plateau monsoon climate with significant vertical climate characteristics. The temperature changes vertically with the terrain, so the map of Yunnan Province is an excellent real-world scene for constructing a simulation environment.
	
	As shown in Figure <ref>, U2USim uses Unreal Engine (UE) 5.2[https://www.unrealengine.com/en-US/unreal-engine-5] to construct a scaled-down 3km*3km simulated environment map based on the map of Yunnan Province. U2USim includes 4 types of terrain: mountains, hills, plains and basins. The elevation range is [56.6, 3000]m. Based on the vegetation distribution in Yunnan, 58 types of original forest vegetation assets were constructed, and more than 15 superposition methods were used to combine them, including epiphytic growth, diagonal staggered growth, and so on. Among them, the leaves of each plant will dynamically change with wind, rain, snow, and other weather conditions. U2USim takes Yunnan Province as a prototype, including 4 terrains, 7 weather conditions, and 8 sensor types. It uses the real meteorological data of Yunnan Province collected by the China Meteorological Center to map the simulation environment based on longitude and latitude.
	
	7 weather conditions. The weather system is designed to simulate the dynamic weather environment in Yunnan Province. It consists of two parts: the practical code and the artistic performance. To create these impressive weather effects, 19 materials and 78 textures were used. To imitate reality, the simulation includes effects that occur from the sky to the ground, such as wet leaves and muddy floors after heavy rain in Yunnan Province. In addition, there are eight highly customized particle systems to simulate rain, snow, dust, and fog at specific positions within the simulation environment. All of these effects are primarily based on Unreal Blueprints[https://www.unrealengine.com/marketplace/en-US/content-cat/assets/blueprints], which can reduce the time required to modify complex code. This allows visual designers to participate more in the programming process and easily change the value for realistic modification.
	
	8 sensor types. The sensors in U2USim are based on the Unreal Engine, which allows cross-platform compatibility and provides accurate data due to the engine's strong physics calculation capabilities. The LiDAR sensor primarily uses ray detection, and with the powerful engine, it can save a lot of time in tracking and calculating each ray. All image-based data, including depth and flare, are generated using Unreal rendering techniques. The flare image is created through the ID segmentation process, where objects are rendered using mapped temperature colors and output. The depth image is captured during the depth test process. The Luminance sensor uses a camera on the drone to capture live textures and calculate the average brightness and smoke concentration of each pixel. The brightness function is based on the color stimulus curve<cit.> of the human eye. The smoke concentration function is based on the Laplacian gradient sum algorithm. To enhance the credibility of the data captured by the sensors, including temperature, humidity and airflow, weather data from Yunnan Province was used to map into the simulator environment (as shown in subsection <ref>). 
	
	These sensors are installed on the multirotor to explore the simulator map and collect data at 0.02-second intervals, which can be customized using a JSON settings file. Sensor mounting is also customizable; users can edit the JSON file to customize their own multirotor by selecting practical sensors from 9 templates and designing the sensor accuracy and return rate.
	
	

 §.§ Real-world mappinng

	We use the real meteorological data of Yunnan Province collected by the China Meteorological Center to map the simulation environment based on longitude and latitude. First, the temperature, humidity, pressure, wind speed, and wind direction data collected by weather stations in Yunnan Province from March 5 to 10, 2024, released by the National Meteorological Information Center are downloaded. The real sensor information is then mapped into the U2USim based on the latitude and longitude information of the real map. Among them, temperature and humidity are scalars, and missing values are filled by the moving average method (interval 5m). For wind speed and direction, assuming that the latitudinal wind speed of a location on March 5, 2024 is u_h m/s and the longitudinal wind speed is v_h m/s, the formulas for the synthesized wind speed v_h and wind direction θ_h are as follows:
	
    V_h=√(u_h^2+v_h^2)

	
    α=arctan(u_h/v_h) ×180/π

	
    θ_h={[               0 (u_h=0, v_h≤ 0);              90  (u_h<0, v_h=0);             270  (u_h>0, v_h=0);           180+α         (v_h>0);           360+α  (u_h>0, v_h<0);               α  (u_h<0, v_h<0) ].

	where α is the angular parameter. The equations for latitudinal wind speed u_h and longitudinal wind speed v_h for missing values are as follows:
	
    u_h=u_1×(H_2-H_1)+∑_i=2 u_i×(H_i+1-H_i-1)+u_n×(H_n-H_n-1)/2 ×(H_n-H_1)

	
    v_h=v_1×(H_2-H_1)+∑_i=2 v_i×(H_i+1-H_i-1)+v_n×(H_n-H_n-1)/2 ×(H_n-H_1)

	where H_h is the potential height of isobars (obtained by sliding average of neighboring sampling points based on distance), n is the number of isobars, and i is the isobar at the missing value location, and the synthetic wind speed and direction are calculated according to Eqs. (<ref>-<ref>).
	
	
	
	
	
	
	
	
	

§ U2UDATA DATASET

	To expedite progress in U2U cooperative perception, we propose U2UData, the large-scale, autonomously flying, real2sim, multimodal dataset with different weather scenarios. This dataset is meticulously annotated with 3D bounding boxes to facilitate research on swarm-UAVs cooperative perception.
	
	Sensor setup. We collect the U2UData using three UAVs flying autonomously in the U2USim. All UAVs are equipped with 5 RGBD cameras (front, back, left, right, and bottom), a 64-LiDAR sensor (top), 1 brightness, temperature, humidity, and smoke sensor (bottom), 2 airflow sensors (back and right), and GPS/IMU systems. Figure <ref> illustrates the sensor layout configuration, while Table <ref> provides a detailed breakdown of the parameters.
	
	Formation mode. The existing UAV flight modes of data collection mainly adopt discipline formation and fixed formation modes. As shown in Figure <ref>, in discipline formation mode, the swarm UAVs maintain a consistent and relatively static array, which has the same height, fixed spacing, and the same speed. In fixed formation mode, each UAV navigates independently with a fixed path, having the same height and speed. However, in a dynamic and open real-world environment, such as UAV delivery tasks, it is difficult for swarm UAVs to maintain a fixed height, speed, and path. In U2UData, we have adopted a new autonomous formation mode in which each UAV flies autonomously. Compared with fixed altitude and discipline or fixed formation mode flying, autonomous flight can more comprehensively explore harsh and complex environments, allowing perception models to achieve higher flexibility and stronger robustness. 
	
	
	
	
	
	Data collection. For the 7 representative weather conditions in U2USim, we manually selected 100 scenarios for each weather condition, including different altitudes and different vegetation coverage, covering a 9 km^2 flight area. For each scenario, we collected 15 seconds of swarm UAVs cooperative perception data. As shown in Table <ref>, we sample the image frames at 30Hz, comprising a total 945K RGB frames and 945K depth frames. We collect 315K liDAR frames at a sampling frequency of 10Hz, including 2.41M annotated 3D bounding boxes for 3 classes. The 945K brightness, 945K temperature, 945K humidity, 945K smoke, and 1.89M airflow values were collected through real-world mapping according to the acquisition coordinates of each image.
	
	3D bounding boxes annotation. For annotating 3D bounding boxes on the gathered LiDAR data, we utilize SusTechPoint<cit.>, a robust open-source labeling tool. We also manually refine the annotations. There are a total of three object classes, including bear, deer, and wolf. For each object, we annotate its 3D bounding box with 7 degrees of freedom, encompassing its location (x, y, z) and rotation (expressed as quaternions: w, x, y, z). The location (x, y, z) corresponds to the center of the bounding box. These 3D bounding boxes are annotated separately based on the global coordinate system of each UAV. This approach enables the sensor data from each UAV to be treated independently as a single-agent detection task. We initialize the relative pose of the two UAVs for each frame using positional information provided by the GPS on both UAVs.
	
	Data usage. In total, U2UData has 315K LiDAR frames, 945K RGB and depth frames, 2.41M 3D bounding boxes, 1.89M airflow value, 945K brightness, temperature, humidity, and smoke values. We randomly divide it into training sets/validation sets/test sets according to the ratio of 0.7/0.15/0.15. It can greatly facilitate the credibility of algorithm performance compared to different papers.
	
	
	
	
	
	
	
	

§ TASKS

	

 §.§ Cooperative 3D Object Detection
 
	Several prior works have demonstrated the efficacy of cooperative perception utilizing LiDAR sensors<cit.>. Nevertheless, whether, when, and how this U2U cooperation using other modalities perception systems has few explored works. In this paper, each UAV first extracts LiDAR modal features through feature encoders and transmits them to adjacent UAVs. In the future, multimodal collaborative perception algorithms can be designed based on this dataset.
	
	
	
	Challenge. In contrast to the single-UAV detection task, cooperative detection presents several domain-specific challenges:
	
		
  * Bandwidth limitation. Conventional U2U communication technologies often operate within narrow bandwidth cons- traints<cit.> and cooperative detection algorithms need to balance between precision and the bandwidth cost carefully.
		
  * Location error. Due to environmental disturbances, inherent errors<cit.> in the adjacent UAVs' relative poses will lead to coordinate system mapping errors.
		
  * Asynchronicity. Heterogeneous UAVs<cit.> are inevitable and the distance between UAVs is variable. Different sensor sampling frequencies and transmission latencies will cause UAVs asynchronous.
	
	
	SOTA methods. We evaluate four categories of outstanding multi-agent embodied cooperative perception methods:
	
		
  * No Fusion. We exclusively utilize the multimodal data from the ego UAV for object detection, establishing this approach as the baseline strategy.
		
  * Early Fusion. Each UAV will transmit raw multimodal information directly to other collaborators<cit.>, and the ego UAV aggregates all multimodal features to its own feature map. 
		
		
  * Late Fusion. Each UAV makes task decisions utilizing its multimodal information and delivers the decision results to others<cit.>, and the ego UAV applies non-maximum suppression to produce the final results.
		
		
  * Intermediate Fusion. Adjacent UAVs employ a neural feature extractor to derive intermediate features, which are subsequently compressed and broadcasted to the ego UAV for cooperative feature fusion. It can fully exploit the advantages of early and late fusion and is suitable for large-scale deployment. We benchmark some outstanding intermediate fusion methods, including When2Com<cit.>, DiscoNet<cit.>, V2VNet<cit.>, V2X-ViT<cit.>, CoBEVT<cit.>, and Where2com<cit.>. 
	
	
	Evaluation. The evaluation area extends by [-100, 100]m in both the x and y directions relative to the ego UAV. Our evaluation metric for UAV detection performance is the Average Precision (AP) at Intersection over Union (IoU) thresholds of 0.5 and 0.7. To assess transmission efficiency, we utilize the average bandwidth, which quantifies the transmitted data size as specified by the algorithm. Consistent with <cit.>, we assess all models under two conditions: 1) Synchronous setting: All UAV feature fusion modules do not consider transmission delay and UAV heterogeneity. 2) Asynchronous setting: All UAV data transmission delays are randomly selected between [0, 150]ms and are interfered by the sampling frequencies of heterogeneous UAV sensors.
	
	

 §.§ Cooperative 3D Object Tracking

	Challenge. Object tracking is a process in which the algorithm tracks the movement of an object. The goal is to track the motion of one or more objects, over multiple frames. This motion estimate changes over time, typically represented by their location, velocity, and acceleration at specific times. In contrast, object detection locates objects within individual frames without establishing associations over time. In cooperative 3D object tracking, multimodal information must be shared among multiple agents while adhering to communication bandwidth constraints to jointly track 3D targets. This task, which differs from traditional single-agent 3D target tracking, presents challenges such as multi-view information fusion between agents, multimodal data fusion, spatio-temporal asynchrony, and limited communication capabilities.
	
	
	
	SOAT methods. In this subsection, we used 9 SOAT cooperative perception algorithms (as shown in Subsection <ref>) to verify the performance of the 3D object tracking task in the U2UData dataset, including No Fusion, When2Com<cit.>, DiscoNet<cit.>, V2VNet<cit.>, V2X-ViT<cit.>, CoBEVT<cit.>, and Where2com<cit.>.
	
	Evaluation. We utilize the same evaluation metrics as outlined in <cit.> for object tracking. These metrics include: AMOTA, average multiobject tracking accuracy; AMOTP, average multiobject tracking precision; sAMOTA, scaled average multiobject tracking accuracy, which ensures a more linear representation across the entire [0, 1] range of significantly challenging tracking tasks; MOTA, multi object tracking accuracy; MT, mostly tracked trajectories; ML, mostly lost trajectories.
	
	
	
	Baselines tracker. We've chosen the AB3Dmot tracker from <cit.> as our baseline. This tracker initially retrieves 3D object detections from a LiDAR point cloud. It subsequently integrates the 3D Kalman filter with the birth and death memory technique to guarantee efficient and resilient tracking performance. It attains state-of-the-art performance while maintaining the fastest speed.
	
	

§ EXPERIMENTS

	

 §.§ Implementation Details

	We designate No Fusion as our baseline. To ensure a fair comparison, all models utilize PointPillar as the backbone for LiDAR feature extraction and use 32x feature compression (decompress) to save bandwidth. Among them, for CoBEVT, we only use the FuseBEVT module for feature aggregation without the SimBEVT module. As shown in Section 4, U2U Data has 315K lidar frames. We randomly divided it into training set/validation set/test set according to the ratio of 0.7/0.15/0.15 and finally obtained 220.5K/47.25K/47.25K frames respectively. During the training phase, we randomly designate one UAV as the ego UAV and train each model until achieving optimal task performance. During testing, we evaluate all compared models using a fixed ego UAV. For the tracking task, we utilize the previous three frames along with the current frame as inputs.
	
	

 §.§ Cooperative 3D Object Detection

	As shown in Table <ref> and Table <ref>, we comprehensively compare synchronous and asynchronous cooperative 3D object detection models on our U2UData dataset. Compared to the No Fusion method, all cooperative perception methods significantly improve detection performance by at least 22.69%/7.40% (AP@IoU=0.5/0.7). Especially in long-range detection, their performance improves by 11.45× and 8.47× (AP@IoU=0.5/0.7). This is because cooperative perception can greatly alleviate the problems of occlusion, sensor performance degradation, and limited perception range of the single UAV. Compared with the Late Fusion method, the Intermediate Fusion method can improve the detection performance up to 15.04%/33.31%(AP@IoU=0.5/0.7). And compared with the Early Fusion method, the Intermediate Fusion method can improve the detection performance up to 11.59%/19.57% (AP@IoU=0.5/0.7), and reduce the communication cost by 84.15%. This is because the Intermediate Fusion method performs multi-scale feature extraction and integration in the feature encoding module before communication and the spatio-temporal cooperation module after communication. It can improve the performance of the perception task and reduce the communication cost in a fine-grained way. 
	
	In the Sync setting, among all the Intermediate Fusion methods, Where2com has the best performance with respect to AP@0.5 and AP@0.7, 0.4% higher than the second best model CoBEVT, 11.59% higher than Early Fusion, and 15.04% higher than Late Fusion. In the Async setting, Where2com has the best performance against AP@0.5, and V2V-ViT has the best performance against AP@0.7. Except for the No Fusion method, the performance degradation of all methods is very obvious when a communication delay is introduced. The results show that asynchrony in cooperative perception tasks is a key issue that needs to be addressed urgently.
	
	

 §.§ Cooperative 3D Object Tracking

	As shown in Table <ref>, we comprehensively compare the performance of the cooperative perception models in 3D object tracking tasks on our U2UData dataset. Compared to the No Fusion method, AB3Dmot combined with cooperative detection significantly improves the tracking performance by at least 8.86% AMOTA and 11.3% sAMOTA. Compared with the Late Fusion method, the Intermediate Fusion method can improve the detection performance by up to 3.97% AMOTA. Compared with the Early Fusion method, the Intermediate Fusion method can improve the detection performance up to 6.8% AMOTA. Similar to the cooperative detection path, CoBEVT and Where2Com achieve the best performance in most of the evaluation metrics. 
	
	

§ CONCLUSION

	In real-world scenarios, UAVs are extremely sensitive to the effects of smoke, temperature, humidity, and airflow due to their small size. Taking fire rescue missions as an example, the presence of red flames and smoke significantly impairs the performance of visual sensors. Brightness and smoke sensors can enhance visual sensors in detecting targets through filtering. Airflow influences the direction in which the fire spreads, while temperature and airflow sensors assist UAVs in planning paths and guiding survivors to safety. However, because these sensors are closely related to terrain and meteorology and have strong interactions between modalities, existing UAV simulators are difficult to collect, and the model trained by existing simulators is difficult to deploy in the real world. We build U2USim, the first real-world mapping swarm-UAVs simulation environment. We present U2UData, the first large-scale cooperative perception dataset for swarm-UAVs autonomous flight. We hope U2USim and U2UData can assist UAVs algorithms in being deployed in the real world.
	
	

§ ACKNOWLEDGMENTS

	
	This work was supported in part by the National Key Research and Development Program of China No. 2020AAA0106300, National Natural Science Foundation of China (No. 62222209, 62250008, 62102222), Beijing National Research Center for Information Science and Technology (No. BNR2023RC01003, BNR2023TD03006), China Postdoctoral Science Foundation under Grant No. 2024M751688, Postdoctoral Fellowship Program of CPSF under Grant No. GZC20240827, and Beijing Key Lab of Networked Multimedia.
	
	
	unsrt
	
	


 Paper Title: Clutter-Aware Spill-Free Liquid Transport via Learned Dynamics.


Load Balancing in Federated Learning
    
   Alireza Javani, and Zhiying Wang
   
                     Center for Pervasive Communications and Computing

  					University of California, Irvine

                     {ajavani, zhiying}@uci.edu
                     

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

empty
empty

§ ABSTRACT 
In this work, we present a novel algorithm to perform spill-free handling of open-top liquid-filled containers that operates in cluttered environments.
By allowing liquid-filled containers to be tilted at higher angles and enabling motion along all axes of end-effector orientation, our work extends the reachable space and enhances maneuverability around obstacles, broadening the range of feasible scenarios.
Our key contributions include: i) generating spill-free paths through the use of RRT* with an informed sampler that leverages container properties to avoid spill-inducing states (such as an upside-down container), ii) parameterizing the resulting path to generate spill-free trajectories through the implementation of a time parameterization algorithm, coupled with a transformer-based machine-learning model capable of classifying trajectories as spill-free or not.
We validate our approach in real-world, obstacle-rich task settings using containers of various shapes and fill levels and demonstrate an extended solution space that is at least 3x larger than an existing approach.






 



























§ INTRODUCTION

The integration of robots is rapidly expanding across various domains, spanning from industrial automation to healthcare, agriculture, and domestic services. The ability to manipulate objects is fundamental for robots to perform a variety of tasks in these diverse domains <cit.>. Among the different categories of objects that robots encounter, liquids present a unique challenge due to their complex behavior. Despite the importance of robotic fluid manipulation in various applications such as handling hazardous materials in laboratories to facilitating assistive feeding technologies designed for differently-abled individuals, research in robotic fluid manipulation is under-explored and challenging for several reasons.



Fluid manipulation is challenging due to the intricate and nonlinear nature of fluid dynamics. Accurately modeling fluid dynamics is heavily task-dependent and requires substantial computational resources, hindering real-time control <cit.>. Existing approaches tackle these challenges by simplifying the complex fluid dynamics into more computationally efficient linearized pendulum or mass-spring-damper models. However, these methods struggle in cluttered scenes due to several reasons. These linearized models restrict the motions to pendulum-like or mass-spring-damper-like behavior <cit.>. Additionally, these approaches impose restrictions on the container's orientation by setting fixed or limited Euler angle ranges throughout the trajectory <cit.>. Moreover, these methods rely on constant monitoring of the liquid surface to execute trajectories, which can be obstructed by opaque fumes produced by the liquid or by obstacles present in cluttered scenes <cit.>. Consequently, the action space is over-constrained, and the robot can only perform limited motions, inhibiting its ability to maneuver around obstacles in cluttered environments.


To address these limitations, we propose Spill-Free RRT* (SFRRT*)[Open-source code and video at: https://avv-va.github.io/sfrrthttps://avv-va.github.io/sfrrt.], for spill-free manipulation of open-top liquid-filled containers, particularly in cluttered environments. Our method enables these advantages (<ref>):
(i) Increased action space, so that the robot can operate effectively in cluttered scenarios. We do this by training a model to implicitly learn liquid dynamics to capture the correlation between container trajectory and spillage risk, 
(ii) No additional information needed. Our method relies solely on the robot and the container shape to operate and does not require constant monitoring of the liquid surface. 


To achieve this, SFRRT* generates spill-free trajectories through these key contributions: (i) it first uses the container shape to inform the generation of a spill-free path via RRT*. By considering the container geometry, the algorithm calculates the maximum tilt angle beyond which the container will spill and constrains the path search accordingly (<ref>). (ii) Subsequently, our method generates a spill-free trajectory from this path by using a time-parameterization algorithm in conjunction with a transformer-based neural network classifier. The neural network, trained on a dataset of container trajectories, predicts whether a given trajectory is spill-free or not, allowing SFRRT* to set the trajectory's jerk to avoid spills (<ref>), (iii) Lastly, to train the transformer-based neural network, we produce a dataset of 700 container trajectories collected from individuals handling different container scenarios.







































































































§ RELATED WORK

Transporting open-top, liquid-filled containers requires the modeling of liquid motion along with the planning of time-parameterized motion paths.

A common approach in liquid modeling relies on using Computational Fluid Dynamics (CFD) simulations to achieve spill-free motions by designing controllers to suppress or limit slosh effects, i.e. liquid vibration effects <cit.>.
Despite the accuracy of CFD simulations, they are computationally demanding and require considerable time to converge, making real-time control applications in robotics unfeasible <cit.>. 

To overcome the computational cost associated with CFD, prior work considers two approaches for modeling the sloshing dynamics inside a container: a spherical pendulum <cit.> and a two Degrees-of-Freedom (2DoF) mass-spring-damper system <cit.>. 
These models are inherently nonlinear and computational constraints necessitate linearization for practical implementation. However, linearization introduces several kinematic constraints that restrict robots' range of motions, thereby limiting their ability to navigate through cluttered scenes.

Spherical pendulums. These approaches present controllers that are designed to prevent liquid spillage <cit.>.

They limit the cup's freedom to only one axis of orientation, while also relying on continuous sensor input to monitor the liquid surface for modeling slosh dynamics. In contrast, our approach dispenses with the need for continuous liquid surface observation and allows the container to retain all its degrees of freedom.
Additional work assumes that a higher-level motion planner already provides an obstacle-free trajectory <cit.>. A quadratic program is then employed to optimize the trajectory within a linearized spherical pendulum dynamics model. While the method eliminates the need for continuous liquid surface monitoring, the linearization restricts object yaw to be constant and allows only tilting the container up to 15^∘ to maintain acceptable linearization accuracy. In contrast, our approach enables motion along all three rotation axes and allows the container to be tilted to significantly larger angles, up to the point of spillage, by modeling the quasi-static fluid dynamics.


Mass-spring-dampers. In these approaches, a sloshing-height evaluation method is employed for time-optimal trajectory planning <cit.>. However, this method is tailored for cylindrical containers subject to 1D and 2D motions. This sloshing estimation technique is extended to accommodate 3D motions but the model assumes that the container remains upright during motion <cit.>. In contrast, our approach allows the container to be tilted on each axis and is compatible with various container shapes.


Finally, a notable limitation of both mass-spring-dampers and pendulum-based slosh dynamics models is their inability to navigate around obstacles consistently. These models rely on maintaining pendulum-like or mass-spring-damper-like motions for spill-free movement, and obstacles can disrupt these motions, rendering these approaches ineffective.


Other Approaches. 
Among other approaches, <cit.> presents an effective method for the rapid, spill-free transport of containers. However, it explicitly states that this technique is not intended for the transportation of liquids, as it does not incorporate constraints to inhibit the accumulation of energy within the cup, which could subsequently lead to sloshing.
Additionally, the method proposed in <cit.> aims to accurately learn slosh dynamics through machine learning, taking into account various liquid properties including thermodynamics. However, its applicability to motion planning with robots has not been evaluated, and its practical implementation necessitates continuous accurate liquid surface monitoring.


Overall, our approach outperforms prior methods by implicitly learning the dynamics of liquid spillage. It does not impose kinematic constraints that would limit the robot's range of motion such as setting a fixed container orientation, thereby enabling its maneuverability in cluttered environments. Moreover, it does not require constant observation of the liquid surface to generate motions and it is not restricted to a particular container shape.


§ METHODS



This section introduces SFRRT*, a motion planner that generates spill- and collision-free trajectories (<ref>).
SFRRT* accomplishes this through two key contributions: a) the informed sampler, mathematically formalized based on container geometry, to efficiently explore the space of spill-free robot states (<ref>), b) a transformer-based classifier that models the underlying nonlinear dynamics of liquid motion and classifies whether a planned trajectory is spill-free or not (<ref>).



 §.§ Mathematical Formulation of the Spillage Scenario


A challenge in spill-free trajectory generation is that the volume of the robot configuration space that contains spill-free trajectories is small and therefore inefficient to explore with random sampling alone <cit.>. This dictates the need for an informed approach to explore dynamically-valid robot states. However, developing a model for combined robot and object dynamics, specifically spillage dynamics, is computationally expensive, task-specific, and impractical for real-world control.
To address this, we need to mathematically formalize how container properties inform the construction of the spill-free space. For example, for a given container that contains a greater volume of liquid, it cannot be tilted as much as when it contains less liquid. Additionally, in scenarios where containers have the same liquid volume, taller containers can be tilted more than shorter ones.


To sample spill-free configurations from the spill-free space, we first calculate the maximum tilt angle that a container can withstand without spilling. Then, we sample states ensuring that their orientation remains within this range. Specifically, under quasi-static conditions, we consider the scenario that involves tilting a container to the point where it cannot retain all its content, i.e., to the angle θ_max, such that any θ_t≤θ_max would be spill-free and any θ_t > θ_max would cause spillage (<ref>). Quasi-static conditions are assumed because they simplify analytical modeling without requiring expensive computational fluid dynamics calculations.




To formalize the spillage scenario, we model the container as a frustum of a cone, which accurately represents most common container shapes and can provide an upper bound on the maximum tilt angle of containers with other shapes, such as round bottom flasks or curvy wine glasses (<ref>).

Specifically, we simplify curved containers to the largest possible frustums of cones that can fit inside the original shape, leading to predicting spillage at a lesser tilt angle than the real container's capabilities. This guarantees a θ less than θ_max, ensuring the generation of spill-free states. Lastly, we simplify the frustum of the cone representation from 3D to 2D, resulting in a trapezoid. This simplification also allows us to represent planar symmetric containers such as rectangular prisms and can be directly extended to 3D. 

To solve for the maximum tilt angle, we assume that the information we have from the container is the bottom circle radius r_b, the top circle radius r_u, the cup height h_c, and the water height h_w (<ref>). With this information, we can derive other relevant variables for calculating the maximum tilt angle. As shown in <ref>, two spillage scenarios could occur: 
[label=(*)]

  * one where the liquid forms a trapezoid and a triangle (<ref>), and

  * another where the liquid only forms a triangle (<ref>).
 This happens because static liquid stays parallel to the horizontal ground and depending on the liquid level, these cases can occur.

To calculate the maximum tilt angle, for both spillage cases, we leverage the key insight that the total amount of liquid remains constant before and after tilting the container. By equating the liquid area in the initial and tilted configurations, we derive an algebraic formula to compute the angle. The resulting equations, <ref> for case I and <ref> for case II, provide the respective maximum tilt angles.


Maximum tilt angle for case I:

    θ_max = tan^-1[h_c(r_u-r_w')/(r_u-r_b)(r_u+r_w')]

where r_w'= h_cr_ur_b - h_cr_2r_b + h_wr_2r_w + h_wr_2r_b/h_cr_2 + h_cr_b.

Maximum tilt angle for case II:

    θ_max =tan^-1[ h_c/r_u-r_b+(2h_w(r_w+r_b)/h_c)]

where r_w= r_u-[(r_u-r_b/h_c)(h_c-h_w)].

The next section explains how these equations are incorporated in the sampling-based path planner to inform the construction of a spill-free path.



 §.§ Informed Geometric Path Planning for Spill-Free Motions


Having formalized the maximum tilt angle θ_max, it is now used to guide the exploration of states that avoid spills, such as an upside-down container, or a container that is tilted beyond its capacity (i.e., > θ_max).
To achieve this, we implemented an informed sampler to sample robot configurations where the container orientation is less than the spill threshold θ_max. Orientation refers to the container's Euler angles, and it is sampled such that the angle between the vertical axis and the axis about which the container is symmetric, is less than θ_max. This informed sampler is then incorporated into a geometric RRT* path planner, to generate spill-free paths. RRT* is chosen due to its asymptotic optimality properties. By preferring transitions with minimal cost, RRT* generates smooth motion profiles that maintain orientation stability <cit.>. 
The next section describes how the path generated by RRT* is parameterized to ensure a spill-free trajectory.



 §.§ Time Parameterization To Create Spill-Free Trajectories



While RRT* generates spill-free paths, to ensure the generation of a spill-free trajectory, we must carefully parameterize the path such that the velocity, acceleration, and jerk, do not cause spillage.

To do this, it is necessary to first model the risk of spillage with respect to the trajectory. To capture this relationship, we employed a transformer-based machine learning model trained to classify whether a container's trajectory is spill-free or not. This model is termed the Spill-Free Classifier (SFC), and utilizes the transformer architecture, as depicted in <ref>.

The input to SFC is the container trajectory and container properties, where the trajectory is the position and orientation over time and the properties represent the container's top and bottom radius, container height, and liquid fill level.
Furthermore, as shown in <ref>, the trajectory is first passed through the transformer encoder layer. Then, the container properties are concatenated with it. The SFC model employs the transformer architecture because its input is sequence data (the trajectory) and transformers have become the leading architecture for handling sequential data due to their efficient parallel training capabilities and long context window <cit.>. 

Lastly, to train the SFC model, we utilized a motion capture system to collect container trajectories (comprised of position and orientation over time) performed by individuals across different container types and liquid volumes (<ref>). We collected a total of 700 trajectories, including 655 human-performed trajectories and 45 trajectories from the Panda robot. These trajectories encompassed a variety of paths from holding cups upside down and following zig-zag patterns to transporting cups in straight or typical paths between two points.



Having modeled the relationship between the container trajectory and spillage risk, we leverage SFC to generate spill-free trajectories. We introduce the Spill-Free Time Parameterization (SFTP), which guarantees the creation of spill-free trajectories by utilizing a time-parameterization algorithm alongside the SFC model.
The key idea is to find the maximum jerk that does not cause spillage. To achieve this, SFTP works by iteratively (line <ref>, <ref>) reducing the maximum jerk of the trajectory (line <ref>, <ref>), querying SFC after each iteration (line <ref>, <ref>) until SFC predicts the resulting trajectory is spill-free. Jerk constraints are especially critical in preventing sudden changes in acceleration that could cause spillage. SFTP only modifies the jerk parameter since it directly affects acceleration and velocity. Lastly,  for time parameterization the Ruckig algorithm <cit.> is used since it can enforce constraints on higher-order derivatives including jerk, acceleration, and velocity.

In summary, to generate a spill-free collision-free trajectory (<ref>), first, the container shape and fill level are leveraged to inform the generation of a path using RRT*. The spill-free classifier (SFC) model is then used alongside the Ruckig time-parameterization algorithm to ensure the generation of a spill-free trajectory. We refer to this combined process of generating the path and the trajectory, the spill-free RRT* (SFRRT*) algorithm. 



§ EVALUATION

We evaluate our spill-free collision-free trajectory planner, SFRRT*, on the 7DoF Franka Emika Panda robotic arm. We design two scenarios and utilize three different containers with different fill levels (<ref>, top row) to quantify the performance of SFRRT*. These scenarios are designed to encapsulate real-world use cases of our motion planner, specifically cases that exhibit a diverse range of robot motions: Scenario 1 (<ref>, left) involves the robot transporting a drink while avoiding the obstacles on the table; and, Scenario 2 (<ref>, right) involves the robot in a chemistry lab transporting a container while avoiding collisions with the fume hood.
Additionally, the containers (wine, flute, and basic glasses) are picked based on their diverse geometries, while the fill levels, ranging from 30% to 80% are chosen to cover tasks with varying risks of spillage.
The objective across our evaluation tasks is to move these containers from the start to the goal states without spilling the contents of the container being transported. Each experiment, as enumerated in <ref>, is repeated five times.


We measure the success rate of SFRRT* (<ref>) and compare its obstacle avoidance performance (<ref>) against the SpillNot method from <cit.>. We also evaluate the accuracy of the SFC model on containers that were not part of the training set, which are also used in the video demo (<ref>) to display a real-world chemistry lab procedure (<ref>). Lastly, we present an ablation study that compares modified versions of the SFRRT* (<ref>). 



 §.§ Implementation Details


The performance of SFRRT* depends on the spill constraint enforced during path planning. This constraint restricts the container's orientation by imposing maximum allowable tilt angles to prevent spillage. As described previously, these angles are calculated using <ref> and <ref>. <ref>, last column, presents the maximum tilt angles for the containers used in the experiments. To train the spill-free classifier (SFC) model, we deployed it on TensorFlow and optimized it using a binary cross-entropy loss function for 400 epochs with a batch size of 32. SFC achieved an accuracy of 94%. Detailed training parameters are available on the open-source code.













Finally, several key parameters are set in SFTP (<ref>). The bounds on velocity, acceleration, jerk and are set according to the robotic arm's kinematic constraints <cit.>. Moreover, the jerk decrease factor rate is set to 2, meaning when SFTP fails to find a spill-free trajectory, J_max will be halved and SFTP will reevaluate for spillage under the updated constraint. This rate selection balances execution time and planning time: a smaller rate would extend the time to find a spill-free trajectory, while a larger rate could lead to unnecessarily slow trajectories due to large jerk constraints. 


 §.§ Success Rate

 To measure the success rate of the experiments presented in <ref>, each experiment was repeated five times and the result is presented in <ref>. Success is defined as the absence of spillage and environmental collisions during the executed motion. Overall, we achieved a 100% success rate for all experiments except experiment 5.
For experiment 5, spill-free trajectories were accomplished in four out of five trials. Experiment 5 faced challenges since 
it used a container with a maximum tilt angle of 35^∘, smaller than others, limiting the trajectory planning options and increasing spillage risk. Additionally, the SFC model, with its 94% accuracy, had difficulty adapting to this constrained scenario, contributing to the lower success rate.






 §.§ Avoiding Obstacles
 


A key capability of SFRRT* is its ability to navigate around obstacles in the environment while ensuring the container does not spill. To analyze this ability, we focus on the end effector orientation allowable ranges. In SFRRT*, the container orientation is constrained by the maximum tilt angle of the container, i.e. it can generate spill-free trajectories, so long as the container is not tilted past the maximum tilt angle.
By enabling the robot to tilt the container to higher angles, it can more flexibly maneuver in cluttered spaces, such as a fume hood, which may require tilting chemistry vessels to fit them inside.


 To illustrate this expanded ability, <ref> shows the maximum possible and maximum utilized tilt angles of all 30 trials of <ref>. 
 In every experiment, SFRRT* utilizes a larger tilt range than the SpillNot <cit.>.
 Notably, SpillNot's tilt angles are not contingent on the fill level and are always limited to a maximum of 15^∘ to maintain low linearization error (<cit.>, Fig 4).
 We also chose <cit.> for comparison because, unlike other methods, it does not rely on sensor-based slosh modeling, and allows translational motion in all axes.


As seen in <ref>, using SFRRT* the robot achieves a maximum tilt angle of 45^∘ when transporting the basic container with a 30% fill level (<ref>, experiment 6), surpassing the SpillNot method's 15^∘. The range of orientation for this specific container is 3x larger than <cit.>. Crucially, the maximum tilt angle for the flute glass with a 70% fill level is 72^∘, a 4.8x increase in orientation range.


Furthermore, it is important to enable orientation changes along all axes to avoid obstacles. For example, maintaining a fixed pitch angle throughout the trajectory prevents avoidance of obstacles in certain cluttered environments. SFRRT* can generate trajectories where all three axes of orientation are utilized. <ref> displays the container orientation over all the 30 trajectories generated for <ref>. As shown, there are changes in Euler angles across all axes, the range for roll, pitch, and yaw angles are [-49, 41], [-30, 30], and [-16, 9] degrees, respectively. In contrast, the SpillNot method imposes a fixed yaw angle throughout the trajectory <cit.>. 






 §.§ SFC: Generalization Capability and Velocity Adaptation


The Spill-Free Classifier (SFC) model is trained using 75% of the 700 trajectories and achieved an accuracy of 94% for the validation set.
To evaluate the generalization capability of the SFC, its accuracy was tested on novel containers not present in the training dataset. Specifically, a round bottom flask, beaker, and graduated cylinder were used to gather multiple trajectories using a motion capture system (<ref>). For each container type, 12 trajectories were gathered with varying fill levels.


As shown in <ref>, SFC demonstrates good generalization capability. It achieves strong performance for the beaker and graduated cylinder, with 11 out of 12 data points accurately predicted. However, its accuracy is lower for the round bottom flask, with only 8 out of 12 data points predicted correctly. This is because the SFC model input for container properties only includes details that capture curvy-shaped containers by estimating them to a frustum of a cone. By representing these shapes as frustums of cones, the SFC model ensures a conservative yet safe portrayal of container properties. While this cautious approach may occasionally lead to conservative false negatives, it guarantees the generation of spill-free trajectories.

We also explore a different model architecture for SFC. The current architecture (<ref>) passes only the trajectory to the transformer layer before concatenating with container properties. We modified this by concatenating properties at each trajectory step before passing it to the transformer. We call this modified architecture SFC_m. SFC_m achieved only 88% accuracy, lower than the 94% of SFC, and demonstrated lower accuracy on novel containers than SFC (<ref>).


Lastly, SFC is able to take advantage of the container shape to generate trajectories with different velocities. As seen in <ref>, considering the same containers, with different fill levels we see that it consistently generates higher velocities for containers with lower fill levels, which are less likely to spill. Moreover, when comparing different containers in the same scenario (as in <ref>), those with higher tilt angles consistently achieve higher velocities. For example, in the comparison between the wine glass and the flute glass, the wine glass consistently achieves lower mean velocities. This is attributed to the geometry of the containers. As shown in <ref>, the wine glass has a lower maximum tilt angle compared to the flute glass, indicating smaller room to maneuver and a higher susceptibility to spillage. Conversely, the flute glass, with its higher maximum tilt angle consistently achieves higher mean velocities.


 §.§ Ablation Study

In this section, two key elements of SFRRT* are modified to evaluate their impact on performance: the informed sampler and the spill-free classifier (SFC) model.


Informed Sampler Variation: SFRRT*_u.
To avoid sampling states such as an upside-down container, or a container tilted beyond its capacity, the informed sampler samples states within the maximum allowable tilt angles for the container (<ref>, last column). To measure its impact, we replace it with a uniform sampler, creating SFRRT*_u. Then, experiments 1 and 2 are executed 5 times on the robot, and the success rate is shown in <ref>. The success rate drops significantly since the path generation lacks state sampling constraints, as a result, the time parameterization algorithm struggles to create a spill-free trajectory using this path.


Spill-Free Classifier Variation: SFRRT*_r.
In SFRRT*, the SFC model assists with time parameterization by selecting the jerk constraints to ensure a spill-free trajectory. To evaluate the impact of SFC, we introduce SFRRT*_r by replacing the SFC model with a random jerk selection. More specifically, a random jerk is selected out of all the jerks that were ever chosen by SFC. The path is then parameterized using that jerk. <ref> shows the success rate for experiments 1 and 2. The observed drop in success rate is due to the random selection of jerk constraints without considering the path itself, in contrast to using the SFC model which takes the path into account when choosing the jerk constraints.



























§ CONCLUSION AND FUTURE WORK

This work presents SFRRT*, a sampling-based motion planner for the spill-free transport of open-top, liquid-filled containers, particularly in cluttered scenes. SFRRT* distinguishes itself by utilizing implicitly learned spillage dynamics via a transformer-based machine-learning approach. This outperforms existing approaches by increasing the success rate in cluttered scenes. Specifically, SFRRT* enables motions across all axes of end effector orientations using the learned model instead of simplified liquid models. It also permits the container to tilt to its maximum tilt angle, thus enabling flexible maneuvering to avoid obstacles. It also does not need visual monitoring of the liquid, which could get obstructed in cluttered scenes. 



However, like many machine learning models, our approach faces a common limitation— the need for a substantial amount of training data. Moreover, containers are approximated as frustums of cones to estimate the maximum tilt angles. This could lead to conservative estimates for containers with more complex curvy shapes. An alternative would be training a model on image inputs to predict tilt angles more precisely. Lastly, circular upside-down motions (loop-de-loop) can avoid spilling if executed with adequate speed. Still, SFRRT* cannot support generating them because it constrains container orientations to be less than the maximum tilt angle. This limitation stems from the fact that SFC is trained on data collected from humans, who typically exhibit caution when handling liquids. Consequently, the model lacks training data to classify loop-the-loop motions as spill-free.


For future work, we are interested in improving the Spill-Free Classifier model to estimate spillage probability rather than solely classifying it, thus allowing for a more precise time-parameterized trajectory. Furthermore, the path planning phase currently only constrains the orientation range within the maximum tilt angle. However, if the sampled orientation in the resulting path approaches the maximum tilt angle, the generated trajectory will be very slow. To address this, we plan to analyze how the sampled orientation affects velocity, acceleration, and jerk during path planning. We aim to generate a time-optimal parameterized path by considering these kinematic factors in the path-planning phase.
Moreover, our approach is versatile and can be expanded to a variety of applications. We plan to extend our method to include spillage dynamics for granular materials such as sand and sugar, to transport a tablespoon of these materials without spillage.
Furthermore, our method is adaptable to different path planners and not solely dependent on RRT*. Moving forward, we plan to evaluate our approach using various planning algorithms and compare success rates and planning times.
Finally, to improve our SFC accuracy and SFRRT* success rate further, we aim to optimize our data collection process by exploring automation through Computational Fluid Dynamics (CFD) frameworks. 






 Paper Title: A User Study Method on Healthy Participants for Assessing an Assistive Wearable Robot Utilising EMG Sensing.





Open-Vocabulary Audio-Visual Semantic Segmentation
    Xianghua Ying
    August 5, 2024
==================================================

empty
empty



§ ABSTRACT 

Hand-wearable robots, specifically exoskeletons, are designed to aid hands in daily activities, playing a crucial role in post-stroke rehabilitation and assisting the elderly. Our contribution to this field is a textile robotic glove with integrated actuators. These actuators, powered by pneumatic pressure, guide the user's hand to a desired position. Crafted from textile materials, our soft robotic glove prioritizes safety, lightweight construction, and user comfort. Utilizing the ruffles technique, integrated actuators guarantee high performance in blocking force and bending effectiveness. Here, we present a participant study confirming the effectiveness of our robotic device on a healthy participant group, exploiting EMG sensing.







§ INTRODUCTION



Human reliance on finger dexterity is paramount for accomplishing everyday tasks involving pinching or grasping various objects. However, individuals affected by neurogenerative disorders, such as strokes or Parkinson's disease, may experience limitations in dexterity levels. In response to this challenge, wearable robotic devices have emerged to assist individuals with such tasks, showcasing increasing success <cit.>. Beyond addressing specific grasping objectives, hand-wearable robots have the potential to offer broader benefits, thereby augmenting human capabilities. These advantages extend to activities such as lifting heavy objects (e.g., moving furniture) and are particularly applicable in industrial settings where human mobility is crucial.













By employing actuators, one can direct fingers into a pinch grasp position and subsequently apply forces to the target object. Extensive research has focused on such actuators, with recent emphasis shifting towards soft robotic variants compared to their rigid counterparts <cit.>. Enhanced safety and comfort for users have been achieved through the increased utilization of soft materials. Notably, pneumatically driven elastomers and textile structures have demonstrated reliability in achieving necessary blocking force and finger bending for routine daily tasks <cit.>. This shift is promising, as wearable soft robotic structures offer superior convenience compared to rigid links and rotary motors due to their lighter weight and safer nature.



Established methodologies exist in the literature for evaluating the effectiveness of hand-wearable assistive devices based on data from user studies. For instance, the Jebsen Taylor Hand Function Test <cit.> and the Box and Blocks Test <cit.> are commonly employed to assess the benefits of assistive devices <cit.>. While these tests prove effective with individuals exhibiting compromised manual dexterity, significant challenges arise when working with such participant groups. Apart from ensuring the safety of robotic prototypes for fragile or vulnerable individuals, complex ethical issues must be addressed. Many researchers, therefore, prefer working with healthy sample groups. However, this introduces complexities, particularly regarding trust, as the natural dexterity of healthy users can influence test results by either aiding or interfering with robotic device actuation. Given the high flexibility and relatively low actuation force of soft robotic devices, utilizing a healthy sample for testing is not advisable. Instead, Electromyography (EMG) sensing should be considered to quantify the assistance provided by the robotic device.

Surface EMG (sEMG) sensors serve as valuable tools for measuring electrical signals generated by muscles during contraction, facilitating insights into muscle activity for various applications in biomechanical research <cit.>, rehabilitation <cit.>, and the development of prosthetics <cit.> or assistive devices <cit.>. In the context of this paper, we focus on using EMG signals to evaluate the effectiveness of an assistive device <cit.>. By measuring the magnitude of the differential between signals at rest and under maximum voluntary contraction (MVC), EMG signals can quantify muscle activity with and without the assistive device, thus assessing its effectiveness.

This paper introduces a novel method validated by sEMG sensors to evaluate the effectiveness of a wearable assistive hand device (Fig. <ref>). We then assess our soft robotic assistive prototype glove using this method and demonstrate its efficacy in assisting individuals in pinch grasping objects. EMG signals are recorded as participants perform a task involving varying levels of pinch grasp force on an object equipped with a force sensor. Our findings shows significant results, validated using linear mixed effect models.








§ THE ASSISTIVE GLOVE
 



To create bending of an inflatable textile actuator, an imbalance between two layers of fabric must be achieved. This imbalance is created by using pleating techniques <cit.>, excess material of one layer is folded and stitched onto the other, and unfolds when inflated. This approach, however, has potential drawbacks, e.g. when the space between the layers is too small for the pleats to unfold (which, for finger sized designs, can become imminent). Another technique to create such imbalance is the use of different types of fabric with varying elasticity. This way, one layer stretches more when actuated and the structure will bend towards the less elastic one. Integrating braided elastic band to the stretch fabric option with the ruffles technique is proven more efficient in terms of blocking force and bending angle capabilities <cit.>.







  §.§.§ Materials
 

To create this textile actuator the two different textile properties layers are selected. Bottom: a plain cotton weave for the (light fabric in Fig. <ref>). Top: a cotton mix with elastane yarn integrated to the weft of the fabric (dark fabric in Fig. <ref>).

To enhance a fabric's stretch behavior and create the desired material imbalance between layers, an additional support material is used that is integrated when assembling the actuators: a braided elastic band, also called elastics, commonly used in clothing to create ruffles or elastic waistbands. It consists of braided polyester and a small part of thin rubber, making it durable and extremely stretchy.







  §.§.§ Fabrication




First, the mono-directional stretch black fabric was cut 80% longer than the cotton bottom layer fabric. The elastic band was integrated on the side seam between the top and bottom layer, first stitched onto the top layer while being stretched, and then sewn onto the bottom layer in a relaxed state. This enabled the top layer to reach an excess length of 180% of the bottom layer. The actuator is equipped with a latex bladder to ensure air tightness.








  §.§.§ Results


Two important parameters for soft bending actuators for wearables are flexion angle and blocking force <cit.>. In example, for rehabilitation or assistive hand exoskeletons it is imperative that each finger is unrestricted in relation to its maximum angle, and actuators should apply 10-15 N blocking force (Fig. <ref>) to the fingers<cit.>. 

In Fig. 2 the elastic band integration boosts both of these critical parameters for the actuator. The force capability increases approximately from 10 N to 25 N, and the maximum bending angle increases from approximately 180 degrees to 360 degrees (Fig. <ref>).



§ ASSESSMENT OF THE ASSISTIVE GLOVE




The existing literature offers established methodologies for evaluating the effectiveness of hand-wearable assistive devices through user studies, utilizing tests such as the Jebsen Taylor Hand Function Test <cit.> and the Box and Blocks Test <cit.>. These assessments are particularly valuable when conducted with individuals whose manual dexterity is compromised in some capacity. However, there are considerable challenges associated with working with such participant groups. In addition to ensuring the safety of the robotic prototype for use by fragile or vulnerable individuals, complex ethical considerations must be addressed.

Given these challenges, many researchers opt to conduct studies with sample groups comprised of relatively "unproblematic" individuals <cit.>. Nevertheless, this approach introduces its own complexities, particularly concerning trust. Healthy or non-compromised users may inadvertently influence test results due to their natural dexterity. Their ability to grasp and hold objects can either override or assist the actuation of the robotic device. This issue is particularly pertinent with soft robotic devices, which possess high flexibility and relatively low actuation force. Consequently, it is not advisable to work with a sample of healthy or non-compromised individuals.

Instead, to accurately quantify the assistance provided by the robotic device, Electromyography sensing should be considered. EMG sensing offers a more objective measure of the user's interaction with the device, allowing researchers to evaluate the device's effectiveness in providing assistance independently of the user's inherent dexterity.



In our latest study, we propose a novel method to evaluate wearable devices with healthy participants using EMG sensing. We then apply this method to assess the effectiveness of our soft robotic assistive prototype glove. Our results demonstrate that our exoskeleton significantly improves participants' grasping ability. We record EMG signals as participants perform a task involving varied grasp force applied to an object with and without the assistance of the robotic system. The benefits of the assistive device are quantified using linear mixed-effects models. As our work is currently under review, we provide only a preview of the results in Figure <ref>.

To evaluate the glove's impact, we analyze the average assistance provided to participants, alongside standard deviations, normalized relative to each participant's maximum voluntary contraction (MVC). Assistance is computed by subtracting the RMS EMG signals recorded with the assistive glove from those recorded without it. Figure <ref> illustrates the muscle contraction required for different grasping task intensities (red) and the corresponding reduction in muscle contraction achieved with the assistive device (green). The mean assistance values for all 20 participants' fingers average around 18% of MVC. Notably, the assistive glove consistently delivers assistance across various task intensities.

These results indicate that the soft robotic assistive glove effectively supplies the majority of the required force for low-force grasping tasks. In a 20N pinch grasp, the glove reduces approximately 70% of the workload on the fingers, with some instances where the robotic device compensates for the entire workload (100%) for certain participants. It is observed that when activated, the assistive glove maintains grasp force assistance ranging from 15 to 23N (depending on hand size). 







§ CONCLUSIONS


 The elastic bands integrated using the ruffles technique proved to be effective in enhancing the performance of the soft robotic structures. In the actuator application, the elastic bands greatly increased the bending capability and force capability of the structure. These findings demonstrate the potential of using elastic bands and textile techniques in soft robotics to create more efficient and adaptable structures.

 

 Given the assistive glove's capability to apply force exceeding 20N on the fingers, our anticipation for more significant results in the user study was reasonable. However, the outcomes were not as expected, mainly due to the inherent nature of wearable devices, which often elevate EMG readings regardless of actual muscle effort. This phenomenon stems from the "unnatural" haptic sensation induced by the device. Analyzing this behavior is crucial for gaining deeper insights into the functionality of the assistive device.

To delve further into this matter, participants were instructed to fully relax their hand on the test rig while the assistive device was activated. This approach allowed us to isolate the signal increase caused solely by the device activation. Our experiments revealed a mean change of 6.5% of MVC in EMG signals upon device activation. For some participants, this change was as minimal as 0.0008% of MVC, suggesting that the EMG alteration cannot be attributed to the static charge of the robotic system. Rather, it reflects the muscle contraction response of the participant to the activation of the assistive glove. Consequently, we can deduce that the assistance provided by the robotic glove exceeds the reported values by approximately 6.5% of MVC. For an extended version of this two page abstract please visit <cit.>.

IEEEtran





 Paper Title: Chance-Constrained Information-Theoretic Stochastic Model Predictive Control with Safety Shielding.










Chance-Constrained Information-Theoretic Stochastic Model Predictive Control with Safety Shielding
    Ji Yin^1, Panagiotis Tsiotras^1, Karl Berntorp^⋆
^1Ji Yin and Panagiotis Tsiotras are with D. Guggenheim School of Aerospace Engineering, Georgia Institute of Technology, GA. This work has been partially supported by ONR award N00014-23-1-2353 and NSF award CPS-2211755. Ji Yin  was an intern at MERL at the time of this research and this support was not applied to the work at MERL.
        {jyin81,tsiotras}@gatech.edu
^⋆ Corresponding author: Karl Berntorp, Mitsubishi Electric Research Labs (MERL), 
Cambridge, MA, USA. Email: karl.o.berntorp@ieee.org.

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

§ ABSTRACT 
This paper introduces a novel nonlinear stochastic model predictive control path integral (MPPI)  method, which considers chance constraints on system states. The proposed belief-space stochastic MPPI (BSS-MPPI) applies Monte-Carlo sampling to evaluate state distributions resulting from underlying systematic disturbances, and utilizes a Control Barrier Function (CBF) inspired heuristic in belief space to fulfill the specified chance constraints. 
Compared to several previous stochastic predictive control methods, our approach applies to general nonlinear dynamics without requiring the computationally expensive system linearization step. Moreover, the BSS-MPPI controller can solve optimization problems without limiting the form of the objective function and chance constraints. 
By multi-threading the sampling process using a GPU, we can achieve fast real-time planning for time- and safety-critical tasks such as autonomous racing. Our results on a realistic race-car simulation study show significant reductions in  constraint violation compared to some of the prior MPPI approaches, while being comparable in computation times.







§ INTRODUCTION


Safety is a pivotal concern in the expanding global robotics industry, directly influencing the range of robotic applications. Hence, the importance of safety in robot applications cannot be understated and it is paramount when humans interact with robots, which operate autonomously, often with unpredictable motions. This unpredictability necessitates stringent safety measures to prevent any potential harm to individuals, especially those unaccustomed to interacting with robots <cit.>.

Over a span of 30 years, 37 robot-related accidents were reported, with 27 incidents resulting in a worker's death between 1984 and 2013. This data underscore the necessity of safety protocols to prevent workplace fatalities <cit.>. 
The evolution of robotic systems from performing static manipulation tasks into more collaborative and dynamic,  further emphasizes the need for robust safety processes <cit.>. 
To address safety and increase robot reliability, it is crucial to consider uncertainties in robot planning and control. Uncertainties, such as dynamical disturbances, can lead to unpredictable robot behavior, which, in turn, might pose risks to human operators or other robots working in proximity. For instance, an autonomous vehicle might fail to respond adequately to unexpected road conditions, endangering the safety of its occupants and other road users.

Many control systems are designed based on deterministic models of the real robot dynamics; these controllers may fail to function as intended due to unexpected disturbances, leading to sub-optimal or even unsafe operations. To this end, during the last decade numerous efforts have been made towards control design incorporating model and environment uncertainties, for example, in a stochastic model-predictive control (SMPC) framework <cit.>. However, the ability to handle nonlinear dynamics and/or non-Gaussian uncertainties in a computationally efficient manner is still a concern.

We propose a novel control approach that considers chance constraints in belief space, using path integral and control barrier functions (CBFs) theories. 
Specifically, we design a heuristic in belief space inspired by discrete-time CBFs (DCBFs) to push the probability of collision lower than a user-specified threshold and integrate it with the Shield Model Predictive Path Integral (Shield-MPPI) of <cit.> to achieve real-time planning. 
We call the resulting control method the belief-space stochastic MPPI (BSS-MPPI). By using Monte-Carlo sampling to evaluate the state distributions, we estimate a general distribution and extract the  first two moments into the state-dependent part of the objective function of the optimization problem, and solve for a solution fulfilling chance constraints for safety. However, BSS-MPPI can also integrate with other, nonsampling based uncertainty-propagation schemes, such as nonlinear Kalman filters. 

Compared to most previous trajectory optimization methods that consider chance constraints, our approach differs in the following ways:

    
  a) 
    First, BSS-MPPI solves a nonlinear stochastic optimal control problem by forward simulation, hence avoiding the computationally-expensive explicit optimization and linearization steps. It can achieve real-time planning on either CPU or GPU by multi-threading. 
    
  b) 
    Second, BSS-MPPI can be applied to more general nonlinear stochastic systems. Previous works that consider chance constraints mostly assume linear/linearized dynamics, linear constraints, and some specific form of disturbances (e.g., Gaussian), which limit the range of applicability in robotic systems.
    
  c) 
    Third, finding a verified nonsaturating CBF (with bounded control input) is difficult for high-dimensional systems <cit.>. We design CBF-inspired heuristics that provide better scalability at the cost of less formal guarantees and apply them to high-dimensional belief space. 
    
 




 §.§ Related Work


Controlling the evolution of state uncertainties to fulfill safety requirements is not a trivial task. 
There are many works in the literature that study motion planning under uncertainties using chance constraints. 
For instance, distributional (e.g., covariance) steering is a recent technique that is used to steer an initial distribution to a target distribution of target mean and covariance values, while subject to chance constraints. 
For unmanned ground vehicles (UGVs)
In <cit.>, the authors formulate covariance steering as a convex optimization problem and use its solution to plan trajectories for linearized vehicle models with additive Gaussian noise in challenging environments. For unmanned aerial vehicles (UAVs), <cit.>  utilize covariance control to achieve path planning with obstacles, and <cit.> uses this technique for altitude control of a rocket.



Other related approaches include <cit.>, which reformulates obstacle avoidance chance
constraints using the signed distance function and applies the sequential convex programming (SCP) algorithm, and <cit.>, which considers the chance-constrained trajectory optimization problem by integrating the constraints into the cost functions and uses dynamic programming to obtain a solution. These approaches all require linearization of the dynamics, or using a discretized state space, which makes real-time planning and replanning computationally challenging, especially for high-dimensional systems.

The authors of <cit.> and <cit.> propose the use of stochastic CBFs (SCBF), which provide 100% safety assurance for a system with Brownian noise in continuous space. 
However, the theoretical guarantees of their approach include an implicit assumption that the control inputs and the dynamical system must be unbounded, which is not practical for many robotic systems.

Compared to many other nonlinear controllers, such as SCP, iLQR, and iLQG, MPPI implements the optimal control problem by forward simulation using the original nonlinear dynamics  and hence avoids the linearizations  involved in the explicit optimal control solvers. MPPI does not restrict the form of the objective function, which can be nonconvex and discrete. However, the base MPPI uses only deterministic dynamics. To improve the robustness of MPPI, many variants of the algorithm have been developed <cit.>.   In particular, <cit.> introduce penalty costs for constraint violations due to dynamical and environmental uncertainties to the MPPI objective function, but they do not provide theoretical guarantee for probablistic constraint satisfaction. The work <cit.> also uses chance constraints, but it assumes linear systems and Gaussian disturbances, hence resulting in Gaussian posterior densities, which are restrictive.






§ SHIELD-MPPI REVIEW
 
Consider a general nonlinear dynamical system,

    x_k+1 = f(x_k, u_k),

where x_k ∈𝒟⊆^n_x is the state, u_k ∈^n_u∼𝒩(v_k, Σ_ϵ) is the control input with the mean control v_k at time step k=0,,K-1 and constant covariance Σ_ϵ. Let ϕ(x) denote the terminal state cost, q(x) the step state cost, and λ the weight for control cost. Then, the Shield MPPI (S-MPPI) controller solves the following problem,

    min_ J()    = 
       𝔼[ ϕ(x_K) + ∑^K-1_k=0(q(x_k)  + λ/2v_k^⊺Σ^-1_ϵ v_k ) ],

subject to (<ref>) with initial condition x_0 = x(0), and the first-order DCBF safety condition,

    sup_v ∈𝒰h(f(x_k, v_k)) - h(x_k) ≥ - p (h(x_k)),

for k = 0, , K-1, where the continuous function h(·): ℝ^n →ℝ is the DCBF that defines a safe set in the state space,

    𝒮{x ∈𝒟| h(x) ≥ 0}.

The function p(·) is a class-κ function that is strictly increasing such that p(0) = 0. 
Fulfilling the DCBF constraint (<ref>) will ensure that the system stays within the safe set 𝒮, and the S-MPPI achieves this by minimizing the violation of (<ref>) using path integral and gradient-based optimizations <cit.>.

Compared to the original MPPI <cit.>, S-MPPI has three major advantages. 
First, the vanilla MPPI requires a significant amount of sample trajectories (normally thousands of samples) at each time step for its success, thus 
often requiring the multi-threading computational ability offered by modern GPUs, which are expensive and generally not available on smaller-size robots. 
On the other hand, S-MPPI typically requires an order of magnitude less trajectories to achieve equivalently satisfactory, and even safer performance. 
Second, S-MPPI is less sensitive to cost parameter variations, saving efforts for cost engineering and allowing for more general, less task-specific cost designs. 
Third, S-MPPI is  safer than the MPPI, in general.

Despite all the attractive properties of S-MPPI, it is still a control approach that uses only deterministic dynamics. 
To remedy this, we introduce a CBF-inspired heuristic to S-MPPI, such that the resulting Belief-space MPPI (BSS-MPPI) can consider model uncertainties and plan in the belief space.



§ PROBLEM FORMULATION


In this paper, the objective is to solve the optimal control problem,


    min_ J() , subject to,
       x_k+1 = f(x_k, u_k, w_k),   u_k ∼𝒩(v_t, Σ_ϵ),
       Pr(x_k ∈ℱ) > 1 - P_fail,  for  k = 0, , K-1, 
       x_0 = x(0),


where the dynamical noise w_k in (<ref>) can take arbitrary forms,  ℱ in (<ref>) is the feasible region, and (<ref>) denotes the set of chance constraints where the probability of violating the constraint is below a specified threshold value P_fail. In Sec. <ref> we explain our proposed method, BSS-MPPI, for solving (<ref>).



§ BELIEF-SPACE MPPI


In this section, we briefly review the MPPI control update law and develop the BSS-MPPI algorithm by integrating the chance constraint (<ref>) using a proposed belief-space heuristic in discrete time.



 §.§ MPPI Control

The novel BSS-MPPI controller has an objective function in the same form as the original MPPI, thus we can use the MPPI control update law. The MPPI algorithm solves an optimization problem by sampling control inputs and forward simulating a large number of trajectories. Assuming that the algorithm has M trajectory samples with prediction horizon K, for the problem (<ref>) subject to (<ref>), the MPPI algorithm has the following control update law,

    ^+ = ∑^M_m=1ω_m ^m/ ∑^M_m = 1ω_m,

where the m^th sample control sequence ^m = {u_0^m, , u_K-1^m }, u^m_t ∼𝒩(v_k, Σ_ϵ),  and where

    ω_m = exp(-1/λ(S_m - β)).

Note that β = min_m=1,…,M S_m in (<ref>) is introduced to prevent numerical overflow, and S_m is the cost of the m^th sample trajectory, given by,

    S_m = ϕ(x^m_K) + ∑^K-1_k=0q(x^m_k) +
         γ (v^m_k)^⊺Σ_ϵ^-1 u^m_k.



 §.§ CBF-inspired heuristic considering chance constraints



  §.§.§ Safe heuristic in belief space

To fulfill the chance constraint (<ref>), we choose a safe set 𝒮⊂ℱ, such that,

    x̅∈𝒮Pr(x ∈ℱ) > 1 - P_fail,  P_fail∈ (0,1],

where x̅ = 𝔼[x]. We introduce the following assumption on the feasible region:









    The feasible region ℱ is defined by the intersection of Z inequalities, such that,

    ℱ ⋂^Z_i=1{x: c_i(x) < 0},   i=1,,Z.
 





Using Assumption <ref> and  the Boole-Bonferroni inequality <cit.>, any state x_k subject to the constraints,


    Pr(c_i(x) ≥ 0)    ≤ p_i,    i = 1,, Z,
    ∑^Z_i=1 p_i    ≤ P_fail.


also satisfy the original constraint (<ref>). From (<ref>), we can approximate each chance constraint by 

    c_i(x̅_k) + ν_i √(η_i^⊺Σ_k η_i)≤ 0,

where Σ_k ∈ℝ^n_x × n_x is the covariance of x_k, η_i = ∇_x c_i(x̅_k), and ν_i is the back-off coefficient <cit.>. We can combine x̅ and Σ and form a belief-space system,

    ẑ   = [     x̅; vec(Σ) ],

where ẑ∈𝒵⊆^n_z, n_z = n_x  (n_x+1). 
Hence, following from  (<ref>) and (<ref>), we obtain the following heuristic,

    h_i(ẑ) = -c_i(x̅) - ν_i √(η_i^⊺Ση_i),

with a superlevel set 𝒮_i containing all states satisfying (<ref>),

    𝒮_i = {ẑ∈𝒵 | h_i(ẑ) ≥ 0 }.

Consequently, the overall safe heuristic for Problem (<ref>) can be chosen as

    h(ẑ) = min(h_1(ẑ),, h_Z(ẑ)),

with a corresponding safe set 𝒮⊆𝒵 being the intersection of all superlevel sets 𝒮_i for i = 1,,Z, given by,

    S = ⋂^Z_i=1 S_i .


The back-off coefficient value ν_i for each deterministic chance constraint is computed to ensure the probability level p_i in the chance constraint. One option is to use the Cantelli-Chebyshev inequality, that is, ν_i = √(1-p_i/p_i), which holds regardless of the underlying probability distribution. However, as a consequence, it may lead to relatively conservative bounds <cit.>. Alternatively, for approximately normal-distributed state trajectories, we can set ν_i = √(2)erf^-1(1-2p_i) where erf^-1 is the inverse error function. The paper <cit.> discusses an alternative, sampling-based approach for constraint tightening.
















































































  §.§.§ Safety condition

Assume the belief-space state follows the system ẑ_k+1 = f_z(ẑ_k, v_k). The safety condition for the heuristic (<ref>) is given by,

    h(ẑ_k+1) - h(ẑ_k) ≥ - p (h(ẑ_k)),

where p(·) is a class-κ function. Satisfying (<ref>) gives us two properties,

    Given an initial condition ẑ_0 ∈𝒮 and a control sequence {v_k}_k=0^∞ such that all (ẑ_k, v_k) pairs satisfy (<ref>), then ẑ_k ∈𝒮 for all k ∈ℤ_≥ 0.


    Let ẑ_0 ∈𝒵∖𝒮 and let
     a control sequence {v_k}_k=0^∞ such that, 
     for all k ∈ℤ_≥ 0, the pair
    (z_k, v_k) satisfies (<ref>).
    Then the
    state z_k converges to the safe set 𝒮 asymptotically.

The proofs of both properties are provided in <cit.>.

If there always exists a control v such that (<ref>) is satisfied for all ẑ∈𝒵, h(ẑ) becomes a discrete-time control barrier function (DCBF). The HJ reachability analysis, which is commonly used to find verified CBFs, however, is computationally intractable for systems of high dimensions <cit.>. For this reason, finding verified CBFs for belief-space systems is difficult in most cases due to large dimensionality. The proposed BSS-MPPI minimizes violation of (<ref>) and achieves an average 98.7% satisfaction rate of (<ref>) in our simulations, effectively keeping system states within the safe set and significantly reduces collision rates.











 §.§ Augmented System


Note that the MPPI objective function (<ref>) and the corresponding control update law (<ref>)-(<ref>) share the same state-dependent step cost q(·), which can be of  arbitrary form. 
To integrate the DCBF with chance constraints into the MPPI objective function and the control update law, we introduce a system in belief space that includes the mean and covariance of the system (<ref>), then augment the new system to combine two states in one, such that the safety condition (<ref>) can be included in the step running cost q(·).


  §.§.§ Belief-space system

We can separate the system state following (<ref>) into the mean and disturbed parts,

    x_k = x̅_k + x_k.

Given a sequence of sampled controls = [u_0, , u_K-1], the mean state x̅_k follows the nominal system,

    x̅_k+1 = f̅(x̅_k, u_k) = 𝔼[f(x_k, u_k, w_k)],

and the covariance propagation evolves according to 

    Σ_x_k+1  = Cov[f(x_k, u_k, w_k)] = f_Σ(Σ_x_k, u_k).

We can then describe (<ref>) using the belief-space system (<ref>),


    ẑ_k+1   = [       x̅_k+1; vec(Σ_x_k+1) ] 
        = [        f̅(x̅_k, u_k); vec(f_Σ(Σ_x_k, u_k)) ]
        = f_z(ẑ_k, u_k).


In some cases, as demonstraded in our simulations, only part of the uncertainty covariance matrix is needed, thus a smaller state space can be used.
For linear systems with Gaussian additive disturbances and for systems with nonlinearities on a specific form, f̅ and f_Σ can be expressed analytically <cit.>.

For nonlinear systems, data-driven approaches, such as neural nets and Gaussian processes <cit.> can be used to model the mean and covariance propagations. In this work, we propose to apply Monte-Carlo sampling to estimate the empirical mean and covariance propagation, such that,

    x̅_k+1 = x_k+1≈1/N∑^N-1_n=0x^n_k+1,

and,

    Σ_x_k+1   = x_k+1x_k+1^⊺
       ≈1/N-1∑^N-1_n=0(x_k+1^n-x̅_k+1)(x_k+1^n-x̅_k+1)^⊺,

where x_k+1^n = f(x_k^n, u_k, w_k^n) is the state of the n^th trajectory sample following the control sequence  at time k+1. 



  §.§.§ Augmented belief-space system


Since the costs ϕ(x_K) and q(x_k) in the objective function (<ref>) are only dependent on the state at a single time step, it is difficult to integrate the safety condition (<ref>) directly, which includes two consecutive system states. 

To this end, we introduce, for each k=1,…, K, 
the augmented state z_k = (z_k^(1),z_k^(2)) = (ẑ_k,ẑ_k-1) ∈^2 n_z and the corresponding augmented belief-space system.

    z_k+1 =
    [ z^(1)_k+1; z^(2)_k+1 ] 
    =
    [ f_z(z^(1)_k, u_k);           z^(1)_k ]
    =
    [ ẑ_k+1;   ẑ_k ].





 §.§ Safety-aware Objective Function

We can apply the MPPI algorithm to the augmented system (<ref>) with cost,

    min_ J() = 
        𝔼[ ϕ_a(z_K) + ∑^K-1_k=0(q_a(z_k)  + λ/2v_k^⊺Σ^-1_ϵ v_k ) ],

to yield a sequence of optimal controls. Note that the step running cost q_a(·) is dependent on two consecutive states of the belief-space system (<ref>). Taking a linear class-κ function, it follows from (<ref>) that,

    h(ẑ_k) - (1-β)h(ẑ_k-1) ≥ 0,

where β∈ (0,1), and h(·) is designed following (<ref>) and (<ref>). The resulting safe condition violation cost is 

    C_safe(z_k) = C max{-h(z^(1)_k) + (1-β) h(z^(2)_k), 0}.

Hence, the state-dependent costs are 


    q_a(z_k) = q(z_k^(1)) + C_safe(z_k),
    ϕ_a(z_K) = ϕ(z_K^(1)) + C_safe(z_K).


For convenience, we set z_0^(2) = ẑ_-1 = ẑ_0, such that C_safe(z_0) = 0. The optimal control sequence can be calculated by (<ref>), (<ref>), and (<ref>), using (<ref>) as the state-dependent running costs. The modified objective function (<ref>) integrates the chance constraint (<ref>) by converting it into a cost minimization problem, which minimizes (<ref>) while penalizing any violations of the safety condition (<ref>) to fulfill (<ref>).
































§ ALGORITHM


Algorithm <ref> gives the pseudo-code for the proposed BSS-MPPI. Line <ref> obtaines the estimated robot state, lines <ref>-<ref> initialize states and trajectory costs, line <ref>-<ref> sample control sequences, and lines <ref>-<ref> propagate the mean states and corresponding covariances. 

In <cit.>, the mean and covariance dynamics are modeled analytically and can be used to directly compute the next states. 
If the system is complex and highly nonlinear, such that the mean and covariance dynamics are difficult to model, lines <ref>-<ref> can be implemented by 
Algorithm <ref>, which utilizes Monte-Carlo sampling to approximate mean and covariance propagation and is generally applicable. 


The disturbance w_k in Algorithm <ref> can be either from a fixed or a conditional (e.g., state-dependent) distribution. 

Lines <ref>-<ref> in 
Algorithm <ref> use the augmented belief space system (<ref>) to evaluate the simulated trajectory costs S^m, and compute the optimal controls using the control update law (<ref>). 
Line <ref> sends the safe control command to the actuators, and line <ref> resets the nominal control sequence for the next control iteration. Fig. <ref> shows the proposed architecture.









§ SIMULATION STUDY


In this section, we evaluate the proposed BSS-MPPI on a simulated vehicle-dynamics racing example, where the objective is to conclude a lap of a race course subject to minimizing a control objective. We develop the CBF-inspired heuristic for the considered application, and carry out a Monte-Carlo simulation study to evaluate the performance of the proposed BSS-MPPI (bss-mppi). We compare it with other state-of-the-art MPPI-based control approaches, in particular, the original MPPI (mppi, <cit.>) and the more recent S-MPPI (s-mppi, <cit.>).



 §.§ Experimental Setup


We use the AutoRally racing platform <cit.> to evaluate our method. 
The AutoRally is a 1m long, 0.4m wide electric vehicle with mass 22kg whose dynamics mimic a real vehicle. 
We model the dynamics using the discrete-time system  (<ref>), based on the planar single-track vehicle model <cit.> sketched in Fig. <ref>. The system state is x=[v_X,v_Y,ψ̇,ω_F,ω_R, e_ψ,e_Y,s]^⊤, where v_X is the longitudinal vehicle velocity, v_Y is the lateral vehicle velocity, ψ̇ is the yaw rate, ω_F,ω_R is the front and rear wheel-speed, respectively, e_Y is the lateral deviation from the centerline, e_ψ is the yaw-angle deviation with respect to the centerline heading, and s is the path coordinate in the road-aligned frame. The control input is u=[δ,T]^⊤ where δ is the steering angle at the front wheel and T is the throttle. The tires are modeled by the Pacejka tire model and we use the friction ellipse to model combined slip <cit.>.

We use the state-dependent running cost 

    q(x^m_k) = (x^m_k-x_g)^⊤ Q(x^m_k-x_g) + 1(x^m_k),

    where Q=diag(q_v_X,q_v_Y, q_ψ̇,q_ω_F,q_ω_R, q_e_ψ,e_y,q_s) is the cost-weight matrix, x_g = (v_g,0,…,0) sets the target velocity, and 
    
    
    1(x^m_k) =  {[                                    0, if  x^m_k  satisfies the constraints,;                                C_obs,                            otherwise, ].

    is the collision penalty cost. Since bss-mppi extends s-mppi by introducing chance constraints into the problem formulation, our evaluation is mostly focused on the ability of  bss-mppi to satisfy the constraints under disturbed, that is, uncertain, dynamics. 
    We have executed 200 Monte-Carlo simulations where the vehicle is tasked to complete a lap of the AutoRally racetrack subject to zero-mean Gaussian process noise, w_k ∼𝒩(0,Σ_w) with the reference velocity v_g=6 m/s. 
    
    To verify the performance sensitivity to the tuning of the objective function, we vary q_e_y in (<ref>) while keeping the other variables fixed. 
    The reason is that the lateral deviation cost q_e_y has a large impact on the maneuvering performance, and thereby it also has implications on the constraint satisfaction. 
    A relatively small q_e_y favors cutting corners while a large q_e_y makes the system stay close to the track centerline. Hence, a large q_e_y reduces the risk of a collision against the track boundaries, but may lead to less efficient trajectories.










 §.§ Belief-Space Control Barrier Function Design


Assuming that the racing track has constant width 2 w_ T, we want to keep the vehicle's lateral deviation e_y from the track centerline bounded by |e_y| ≤ w_ T for some given collision probability δ.
Hence, we can write the chance constraints as


    P(e_y < w_ T) > 1 - ϵ,
    
     P(e_y > -w_ T) > 1 - ϵ,


which keeps the chances of collision avoidance for each track boundary above 1-ϵ. 
 The chance constraints (<ref>) are equivalent to


    P(e_y ≥ w_ T) ≤ϵ, 
    
     P(e_y ≤ -w_ T) ≤ϵ.


It follows from (<ref>), (<ref>) that (<ref>) can be converted to deterministic chance constraints,


    e̅_y - w_T + νσ_y ≤ 0, 
    e̅_y + w_T -  νσ_y ≥ 0,


where σ_y is the standard deviation of e_y, obtained from the Monte-Carlo covariance propagation (see Algorithm <ref>).
Since (<ref>) and (<ref>) are symmetric, we can combine them to form a single inequality,

    |e̅_y| ≤ w_T - νσ_y.

Hence, we can formulate the  DCBF

    h(x) = (w_T - νσ_y)^2 - e_y^2,

and the corresponding safe set is (<ref>). We implement the chance constraints with the back-off coefficient ν = √(2)erf^-1(1-2ϵ) (see Remark <ref>), which for the considered application turned out to be a good compromise between conservativeness and safety.



 §.§ Simulation Results


To illustrate, Fig. <ref> shows  the trajectories for a set of runs produced by s-mppi and bss-mppi when trying to manuever a lap of the race track. The trajectories produced by s-mppi have a tendency to go close to the track boundaries and on several occasions also cross the track boundaries, indicating  a subsequent crash. In contrast, owing to the chance constraints, bss-mppi trajectories tend to push closer to the track center, thus   yielding safer trajectories.


To quantify the results, we have executed  the different controllers using different tuning parameters, and for each set of parameters, we have executed the scenario in Fig. <ref> for 100 Monte-Carlo runs using different noise realizations and initial conditions.    Table <ref> shows the number of crashes and collisions, and the corresponding  ratios for different number of trajectories for mppi, s-mppi, and bss-mppi, respectively. We define a collision to occur when the vehicle deviates from the middle of the lane with more than 1.8m (which defines the chance constraint for the lateral deviation), and similarly we define a crash to occur when the vehicle goes outside of the (assumed constant) lanewidth w_ T=2m from the center of the lane, indicating a severe constraint violation. With this notion, we can quantify the number of constraint violations, in addition to quantifying the number of times the controllers heavily violates the constraints. 

Irrespective of the number of trajectories used, bss-mppi experiences substantially fewer crashes and collisions throughout the 100 Monte-Carlo runs. The vanilla MPPI (mppi) crashes at almost all  of the 100 Monte-Carlo runs and has on average 2.3 constraint violations per lap. The number of crashes and collisions for s-mppi is more or less constant irrespective of the number of trajectories, while the safety of bss-mppi improves as the number of trajectories involved to approximately solve the optimal control problem increases. With a sufficiently large MN, the collisions and crashes almost diminish. Even if the number of trajectories used to approximate the covariance propagation N is large, bss-mppi is not able to recover appropriately if the number of sampled control sequences M is small. 



To illustrate the sensitivity to cost function designs, Figs. <ref> and <ref> display the crash and collision rates for different values of M and N as a function of q_e_y (the lateral deviation cost term) along with the 1-σ confidence interval. It is clear that  bss-mppi, the crash rate is relatively  insensitive to the value of q_e_y, while for  s-mppi with both M=5000 and M=20,000, the crash and collision rates generally reduce with increasing q_e_y. This is because s q_e_y increases, the lateral deviation term dominates the cost function and for q_e_y=40, the controller tries to stay in the middle of the track and does not allow for closely tracking other objectives of the cost function. Furthermore, using MN=5,000 in bss-mppi in general leads to fewer crashes and collisions than s-mppi except for a very large penalization on the lateral deviation. However, again, such large penalization leads to other objectives of the cost function to  be ignored.
We can also see that the difference in crash rates between using M=5,000 and M=20,000 in s-mppi is small. Since  s-mppi does not account for any uncertainty, increasing the number of sampled control sequences M will only help marginally in improving performance.



 

Fig. <ref> displays heatmaps of the crash and collision rates  to traverse a lap of the track using bss-mppi with different number of sampled control sequences M and uncertainty  evaluation trajectories N. It is clear that increasing N leads to fewer crashes, as the belief space trajectories and chance constraints are better approximated. However, only increasing N is not sufficient, since using a small M implies limited exploration of the optimal controls. Hence, the optimal choice is a trade-off between control exploration, uncertainty propagation, and computational resources.





 §.§ Results for an Obstacle-Cluttered Track


In this section, to evaluate the proposed method's ability to perform collision avoidance we include obstacles placed on the track. For simplicity, we use static obstacles. 
However, conceptually it is straightforward to also include moving obstacles. 
The obstacles are circular with radius of 0.8 m, and to make the problem challenging, we place them close to the paths taken by the MPPI variants for the obstacle-free case (see Fig. <ref>). 
Hence, the same trajectories as computed in the obstacle-free case will now lead to collisions with obstacles. 

Fig. <ref> shows the paths taken by s-mppi and BSS-MPPI, respective,  for a subset of the 100 Monte-Carlo runs, as well as the obstacles along the track. 
The paths generated by BSS-MPPI are, similar to the ones in Fig. <ref>, more concentrated, that is exhibiting less variation among the different runs. 
Interestingly, depending on the disturbance realizations for a partiuclar run, there is a bimodality of the trajectories close to several of the obstacles. 



Table <ref> displays the corresponding crash ratios for different combinations of q_e_y, N, and M/ WRite oout results based on last simulations




§ CONCLUSION
 


We presented a novel MPPI controller that accounts for system uncertainty by leveraging a CBF-inspired heuristic to satisfy chance constraints. The method can handle nonlinear dynamics and solves the underlying nonlinear stochastic optimal control problem by forward simulation of control and uncertainty trajectories, thus avoiding explicit linearization and optimization steps. The results as verified on a high-fidelity simulation environment indicate that our method effectively reduces the number of constraint violations, while at the same time achieving computational times comparable to previous MPPI approaches. 

In the future, we plan to evaluate the proposed method on an experimental setup, as well as extend it to perception-aware control.












































  














































IEEEtran





 Paper Title: Analyzing the impact of semantic LoD3 building models on image-based vehicle localization.




































	1 Professorship of Photogrammetry and Remote Sensing, 
    2 Chair of Engineering Geodesy, 

 TUM School of Engineering and Design, Technical University of Munich, 80333 Munich, Germany 
 - (antonia.bieringer, olaf.wysocki, christoph.holst)@tum.de

 3 3D Mapping Solutions GmbH, 83607 Holzkirchen, Germany - sebastian.tuttas@3d-mapping.de

4 Hochschule München University of Applied Sciences, 80335 Munich, Germany - ludwig.hoegner@hm.edu






















Numerous navigation applications rely on data from global navigation satellite systems (GNSS), even though their accuracy is compromised in urban areas, posing a significant challenge, particularly for precise autonomous car localization.

Extensive research has focused on enhancing localization accuracy by integrating various sensor types to address this issue.
This paper introduces a novel approach for car localization, leveraging image features that correspond with highly detailed semantic 3D building models.

The core concept involves augmenting positioning accuracy by incorporating prior geometric and semantic knowledge into calculations. 
The work assesses outcomes using Level of Detail 2 (LoD2) and Level of Detail 3 (LoD3) models, analyzing whether facade-enriched models yield superior accuracy. 
This comprehensive analysis encompasses diverse methods, including off-the-shelf feature matching and deep learning, facilitating thorough discussion. 
Our experiments corroborate that LoD3 enables detecting up to 69% more features than using LoD2 models.

We believe that this study will contribute to the research of enhancing positioning accuracy in GNSS-denied urban canyons. 
It also shows a practical application of under-explored LoD3 building models on map-based car positioning.




Analyzing the impact of semantic LoD3 building models on image-based vehicle localization
    Antonia Bieringer1, Olaf Wysocki1, Sebastian Tuttas3, Ludwig Hoegner4, Christoph Holst1,2
    
=============================================================================================






 





§ INTRODUCTION

Navigating in complex urban environments can pose challenges for positioning systems, primarily when vehicles depend on GNSS. As vehicles move through signal-obstructed urban canyons, the effectiveness of positioning systems can diminish. 
Consequently, vehicles shall turn to alternative cues to determine their location in such scenarios.

Various methods have been developed to address this issue, combining different sensors to enhance location accuracy. 
While GNSS enables global positioning, supporting it with local cues is an intuitive choice. 
For example, directly enhancing GNSS signals by using 3D city models <cit.>.
More frequently, however, the researchers have focused on utilizing cameras for enhanced positioning owing to their abundance on cars and optical features.





In order to localize the vehicle with optical cameras, features have to be found in the images. There are various techniques for feature finding and matching, e.g., SIFT <cit.>, SURF <cit.>, or ORB <cit.>. 
Over time, researchers compared those methods against each other <cit.> and improved them.
However, to globally position the images, a reference 3D map with the correct scale and position shall be used.

Such 3D map can be extracted directly from governmental and proprietary geoportals <cit.>, where semantic 3D building models are available.
These models are used in different Level of Details (LoD). LoD1 models are limited to geometrical information that includes the vertices and edges of the buildings; they are cuboid polyhedral models with flat roofs. 
LoD2 models are more detailed and include the generalized structure of the roof. 
In comparison, LoD3 is enhanced by facade details such as windows and doors <cit.>. 

A few papers have already investigated the usage of LoD1 and LoD2 models for vehicle localization.



One of the examples of utilizing LoD1 and LoD2 models is the paper of Vogel et al., who introduce a localizing approach in the inner-city outdoor environments using 3D city models <cit.>. 
To this end, they modify an iterated extended Kalman filter to be able to integrate additional information into it, e.g., available maps. 
A more reliable and precise georeferencing is achieved since geometric circumstances are considered <cit.>. 
Their idea is refined in later papers of <cit.>.
However, the usage of LoD3 models facade features for localization has yet to be investigated.


Our contributions are as follows:


 
  * Map-supported vehicle localization by using high fidelity LoD3 models and images.
 
  * Harmonizing two distinct modalities: Visual information encapsulated within optical images and the structure of semantic 3D models' non-textured surfaces.
 
  * Comparing the benefits of LoD3 over LoD2 building models for camera localization.




§ RELATED WORK


We devote our work to analyzing the impact of image-based vehicle positioning using semantics of 3D maps. 
Therefore, in this Section, we present the state-of-the-art in semantic 3D city models and image- and map-based positioning.



 §.§ Semantic 3D city models


At city, regional, and national scales, semantic 3D city models offer comprehensive representations of structures, taxonomies, and aggregations.
Semantic 3D city models are frequently described by the internationally adopted CityGML standard, established by the Open Geospatial Consortium (OGC) <cit.>; 
This data model standard adopts two encodings using either Geography Markup Language (GML) or CityJSON <cit.>.
The CityGML standard enables the modeling of urban objects with 3D geometry, appearance, topology, and semantics across four different Levels of Detail (LoD). 

Building descriptions are pivotal to semantic 3D city models due to the foundational role of urban dwellings in cities <cit.>. 
LoD1 and LoD2 building models, currently prevalent, boast around 220 million models available in countries such as Germany, Japan, the Netherlands, Switzerland, the United States, and Poland[1].
While LoD1 building models are represented by prismatic 3D models of height-extruded building footprints, LoD2 models additionally represent complex roof structures.     
The widespread adoption of these models is attributed to robust 3D reconstruction algorithms and the availability of building footprints combined with aerial observations <cit.>.

However, LoD1 and LoD2 models lack a more detailed description of facade elements.
Addressing this gap are LoD3 building models featuring descriptive facades with objects such as windows, doors, balconies, and underpasses <cit.>.
The current practice shows that these highly-detailed objects are manually modeled <cit.>. 
Yet, a great deal of research has been devoted to enabling automatic LoD3 reconstruction, which has sparked advancements in the LoD3 data adoption
[<https://github.com/OloOcki/awesome-citygml>].


The recently developed LoD3 reconstruction methods promise to increase the availability of such models <cit.>.



 §.§ Image-Based Vehicle Positioning





One way of reconstructing the 3D structure of a scene and estimating the camera poses from a collection of 2D images is Structure from Motion (SfM) <cit.>. 
This technique works by leveraging the relationships between features in different images and the scene's geometry to recover the scene's 3D structure and camera pose.

Another approach of estimating the vehicle's position by using a series of camera images is Visual Odometry (VO). 
It is advantageous over other methods due to its insensitivity to soil mechanics and lower drift rates <cit.>. 
Additionally, it performs well in places where a satellite connection is limited, e.g., indoors or on narrow streets. 
However, the trajectories estimated by visual odometry tend to drift along with increasing distance from the start point. Consequently, researchers investigated to improve the concept by expanding VO ideas to another image-based localization method called SLAM <cit.>.

Simultaneous Localization and Mapping (SLAM) reconstructs reality from observation data, e.g., images and points clouds. Compared to SfM and VO, it computes the trajectory in real time and maps the vehicle's environment <cit.>. The map is updated while driving <cit.>, and consequently, the current point is automatically corrected by previously calculated results. 
A prerequisite for the map-supported SLAM are surroundings that are easy to detect and can be used to scale and correctly map the trajectory <cit.>. 
As Lucks et al. show, the prior 3D building map can enhance the SLAM-based calculated trajectory <cit.>. 



§ METHODOLOGY

In Figure <ref>, the workflow of the developed method is shown. 
First, virtual pictures of the building models are created. The resulting images are then used to find features in the corresponding optical images. 
For this purpose, the ORB detector <cit.> is used. The generated 2D-image coordinates are correlated with a 3D coordinate by benefiting from the LoD model semantics and geometry. In the end, these coordinates are needed to estimate the camera position for every single image with spatial resection. 
This workflow is designed for LoD1, LoD2 and LoD3 building models. 
The developed method's code is published in the GitHub repository[<https://github.com/tum-pf/LoD3ForLocalization>].





 §.§ Creating Virtual Images of the LoD Models


For the first step, the virtual images are created by using ray casting. To work with the ray casting algorithm, a polyhedral building model is tranformed into a mesh model using triangulation since all results are based on the hit of a triangle by virtual rays. 
The algorithm creates those virtual rays and sends them through a virtual camera, preferably in the direction of the model. Every time a ray hits a point, the corresponding triangle and the barycentric coordinates of the exact hit point in the triangle are saved. Therefore, the 3D coordinates in the real world can be calculated by only knowing the 2D coordinates of the virtual image. The calculation is described in Section <ref>.

Moreover, the virtual camera's pose is required (see Figure <ref>). As an approximation solution, the position is taken from the GNSS data of the current corresponding optical image. 
On top, the camera's height above the GNSS antenna must be considered, leading to GNSS values adapted to the camera position. 
The camera is pointing to the adapted GNSS position of the consecutive image. 
Afterward, the orientation of the camera requires to be adjusted: The angles for roll, pitch, and yaw are required to approximate the point close to the GNSS position in consideration of the camera tilt. 
A visual example of pitch can be seen in Figure <ref>. The approximation is calculated as follows:

    dy [m] = r_GNSS· dy  [^∘] ·π/180 ^∘


An overview of the above-described virtual setup can be seen in Figure <ref>.


After applying the ray casting algorithm, the results are available in a tensor. There are five results: the distance, the geometry IDs, the primitive IDs, the primitive normals, and the barycentric coordinates. For an example of LoD2 combined with one LoD3 building, see Figure <ref>:


From these results, the primitive-normals-data is saved as an image. As shown in Figure <ref>d, some walls of the buildings have the same color as the background. To make every plane of the building visible, the values of the primitive normals are adjusted so that only the absolute values are considered since the exact value is unimportant for the virtual images.
This adjustment is necessary so that every side of the building has a unique color. Otherwise, some buildings' planes will not be considered during feature matching. 
Note that the LoD3 building in the virtual image in Figure <ref> is more detailed due to the additional information on the model, especially regarding windows and roof eaves.









 §.§ Feature Matching


An overview of the approach is visualized in Figure <ref>. The feature matching is performed on images with reduced details. Therefore, the images become more abstract and only focus on important information. 
Then, the images are matched against themselves: the virtual and the optical image. 

The next step is done for the image of the real world, as well as for the one of the virtual camera. 
First, another abstract, virtual feature image is created and implemented as a matrix with the same values for every pixel. If features were found in matching the images against themselves, those pixels are colored differently. This is realized by assigning a different value to this pixel in the matrix. The image, therefore, only contains information about the essential points of the image before. 

Afterward, these newly created feature images are matched with an ORB-feature detector again. 





 §.§ Correlating the Features with a 3D-Coordinate
 

During ray casting, every hit of the mesh by a ray is saved. 
In this case, the corresponding triangle IDs are stored. 
Then, the corresponding triangles in the mesh are found, which enables the extraction of which vertices belong to a triangle that was hit. 
From the IDs of those vertices, the 3D coordinates from the vertex positions are extracted in the mesh.
Finally, the exact point needs to be weighted by the vertices of its triangle because only the relative positions from the vertices are known <cit.>. Those relative positions are named u, v, s. 
The results only return two barycentric coordinates for each hit point since the sum of the three coordinates equals one:

    1 = u + v + s


This means that s can be directly derived from u and v.
The weighting to receive the final coordinates works as follows:

    P = u*P_vertex_1 + v*P_vertex_2 + s*P_vertex_3




 §.§ Calculating the Trajectory


The vehicle's trajectory is simultaneously calculated by driving and reading the generated images. To this end, we leverage the spatial resection concept. Spatial resection is a set of statistical formulas to compute the accuracy of observations by finding a model that fits them best. Consequently, the accuracy of the observations and related values can be calculated afterward. In this case, the camera position is estimated while the observations are optimized at the same time. The position calculation is performed with the help of the photogrammetrical co-linearity equations, which can be seen in <ref> and <ref>.


    x = x̂_̂0̂ + z (r̂_̂1̂1̂(X-X̂_̂0̂) + r̂_̂2̂1̂(Y-Ŷ_̂0̂) + r̂_̂3̂1̂(Z-Ẑ_̂0̂)/(r̂_̂1̂3̂(X-X̂_̂0̂) + r̂_̂2̂3̂(Y-Ŷ_̂0̂) + r̂_̂3̂3̂(Z-Ẑ_̂0̂)


    y = ŷ_̂0̂ + z (r̂_̂1̂2̂(X-X̂_̂0̂) + r̂_̂2̂2̂(Y-Ŷ_̂0̂) + r̂_̂3̂2̂(Z-Ẑ_̂0̂)/(r̂_̂1̂3̂(X-X̂_̂0̂) + r̂_̂2̂3̂(Y-Ŷ_̂0̂) + r̂_̂3̂3̂(Z-Ẑ_̂0̂)


For the calculation, six values are to be determined, representing the camera's position (X_0, Y_0, Z_0) and orientation (ω, ϕ, κ).
Consequently, for the solution, at least six corresponding points are required. More corresponding points are preferred to get even better accuracy. 

The spatial resection is performed as follows. A direct linear transformation generates the approximate camera orientation values. Input parameters are at least six corresponding points. The approximate camera position is taken from the corresponding GNSS point. The approximate values are required because the spatial resection needs starting points to optimize. Otherwise, the algorithm might run into a local minimum and keeps iterating in an infinite loop. 

In the optimization loop, the derivations of the A-matrix are computed. This is necessary because the refinements of the observations for the best-fitting model are calculated with the help of this matrix. The A-matrix has the size of [n x m], while n is the number of observations and m is the number of values to be determined.

The values that are to be determined are estimated in every iteration by calculating the following:

    δx̂ = (A^T*P_bb*A)/(A^T*P_bb*w)

Where A is the already described A-matrix. P_bb is a matrix that contains weights. Those weights decide how important an observation is, e.g., if the measurement is probably more precise than others because of the geometrical position. The w-vector contains the current errors between the approximated δx̂ and the actual observations. The optimized observation values are computed by:

    δv̂ = A*δx̂ - w




§ EXPERIMENTS
 


The developed method is evaluated in three different test areas, which are visualized in Figure <ref>. 


Notably, there were no instances in the test area where LoD3 models were present on both sides of the street, which mimics a frequently present real-world situation. 
The missing LoD3 models were supplemented from the LoD2 dataset as a solution. 
Therefore, the same models from the LoD2 dataset were used for experiments involving solely LoD2 models. 
This hybrid approach ensures a balanced distribution of potential reference points and promotes an effective configuration. 
Owing to the experimental setup, we were able to directly evaluate the influence of the LoD3 over to the usage of only LoD2 models.

The test site was located in Munich, Germany, and represented a Central European architecture comprising a mixed residential area and a university campus.
The images were acquired by a mobile mapping vehicle of 3D Mapping Solutions GmbH <cit.>.
The LoD2 models were downloaded through the governmental open-geodata portal of Bavaria, Germany <cit.>, whereas the LoD3 models were developed within the interdisciplinary tun2twin project <cit.>.




 §.§ Different Approaches for Feature Matching


The feature matching was performed on five different pillars. Each approach is described in the following subsections.



  §.§.§ Corresponding Image-Pairs of Different Types


 


This approach aims to find features from image pairs that are not the same type. This means that one virtual image was matched with one image of reality. Both images were taken from the same GNSS point. The results show only a median of 9-18 suitable feature matches for each image pair. 



  §.§.§ Segmented Images with Deep Learning
 

 


Matching real images with virtual images presents a challenge due to disruptive elements not being shared between the two image types. To mitigate this issue, the approach involves segmenting the real images. 
The used algorithm was built on the SegFormer network <cit.>, using the MMSegmentation framework <cit.>. 
This algorithm generated an output image where distinct elements (e.g., buildings) were each assigned a unique color.


To match these semantically segmented images, a method is devised wherein the segmented images act as masks for the real images to isolate the building areas from the real images. 
Consequently, only the building-related parts of the real image are retained and matched with their virtual counterparts. 
This strategic approach effectively minimizes any potential disruptions from extraneous objects that could potentially result in erroneous matches.



  §.§.§ Sobel-Filter and Canny Edge Detector


 


An established approaches for feature matching focuse solely on features along object edges, in this case, including structures such as window edges, e.g., Sobel filters and Canny edge detector <cit.>. 

In instances where it was desirable to exclusively match buildings, applying a bounding box to actual images proved effective. 
These filters were harmonized with the segmented images detailed in Section <ref> to ensure precision. 
This strategic combination ensured that only relevant objects for vehicle localization were considered. 



§ RESULTS AND DISCUSSION


The experiments were evaluated by comparing the number of found features and the standard deviation of the camera position. 
For every table, the corresponding gain was computed as a percentage increase or decrease between LoD2 and LoD3. 



 §.§ Number of found features


The different approaches led to different results regarding the number of found features, which represented those that can be successfully matched between two images. 
The selection pertains to the median of all tested images within each area. The best result for each experiment is written in bold, comparing LoD2 and LoD3. An example of the found features in the images is visualized in Figure <ref>.



A comprehensive comparison for each model, namely LoD2 and LoD3, is presented across different test areas in Tables <ref>, <ref>, and <ref>.







As our experiments corroborate, the LoD3 models, on average, result in the same or more found features between the images. We deem the feature images algorithm as the most reliable feature extraction between semantic 3D building models and optical images, as it scored more features on average in the three test areas (60%, 69%, and 58%, respectively). 

In the test area 1 with prevalent LoD2 models, the algorithm matched features for LoD2 until the point where the whole virtual image was facing the model-absent underpass. 
Then, no feature matching was possible again and the algorithm failed.
This happens due to the frequently absent underpasses in LoD2, where instead a vertical wall is rendered to the ground level, as seen in Figure <ref>.



Especially in test area 2, more features lead to a more reliable estimation of the trajectory point: up to 69% more features (see Table <ref>). 

As described in Section <ref>, test area 3 exhibited the optimal situation for localizing the vehicle with the help of high-detail polyhedral models. 
This can be seen in Table <ref>, where 57% more features were found, and seven of the tested methods achieved an increase of found features.
We also observe that the higher the buildings, the more details can be found for feature matching.
Furthermore, narrow streets are advantageous because the buildings are closer to the camera and can be captured in more detail. 
In contrast, GNSS data is most accurate in urban environments when surrounded by only low buildings; narrow streets lead to positionining deviations. 
Consequently, the localization with images and LoD3 models can be advantageous over using GNSS in urban areas. 
Both methods are like counterparts: where one method fails, the other has advantages.



 §.§ Positioning accuracy


To analyze the advantage can be achieved by LoD3 models, Tables <ref>, <ref>, <ref> show the accuracy of the estimated camera position in comparison.







Regarding test area 1, the accuracy of the feature images algorithm drops to [-37%, -202%, -26%]. This shows, that the algorithm is not yet working perfectly for every situation. The configuration with the underpass leads also in LoD3 to problems in the accuracies. Still, other methods show the advantages of LoD3 also in this case. The tackling of the problems is described in Section <ref>.

The developed algorithm works less accurately in urban areas when buildings are available on only one side of the street, see Tables <ref> and <ref> (test area 2), especially for the Feature images algorithm. 


As seen in Tables <ref>, <ref> and <ref>, LoD3 reaches a higher number of features. More found features provide a better distribution in the image and, therefore, result in a better standard deviation of the camera point, e.g., up to [88%, 70%, 79%] more accurate in test area 3 (see Table <ref>).  
Considering the standard deviation of the camera position for the evaluation, LoD3 is favorable in test area 3.



 §.§ Limitations
 
The developed algorithm shows the advantages of using LoD3 over LoD2 models. Still, the deviations of the estimated point from the corresponding GNSS point are large. 
To use those models for positioning, the feature matching has to be optimized so that matched features refer to exactly one single point in the real world. 
This is not the case for all matched features so far.

In general, the deviation of the calculated camera position performs slightly better in LoD3 compared to LoD2. 
A reason for the deviation from the GNSS point is an imprecise calculation of the 3D coordinates. 
The exact coordinates are weighted by the barycentric coordinates in the triangle. 
Consequently, each triangle vertex belongs to exactly one of the barycentric coordinates u, v, s. 
This depends on the sequence in which the triangle coordinates are saved in the mesh. The order of the vertices is a requirement currently investigated for LoD2 and LoD3 models. 
The topology for such models has to be consistent in every model, e.g., in the right-hand rule. 
This is not yet common for all developed LoD3 models since the automatic generation remains in its infancy and is prone to manual, randomly induced errors. 
These errors lead to an imprecise calculation of the 3D coordinates and, therefore, to a deviation of the calculated camera position from the GNSS point. 
The method developed in this paper works assuming that the models are correct and the vertices are saved in the right-hand rule. 



§ CONCLUSION


In this paper, we propose an approach to compare the performance of LoD2 against LoD3 models for map- and image-based vehicle positioning. 
Using a feature image method, we harmonize semantic 3D city models with optical images. 
Our experiments corroborate that although we had different representations, optical images vs. quasi-optical images of the LoD models, our method performs well in feature matching. 
The presented method performs best when buildings cover both sides of the street.
We also observe that the higher the building models, the more details can be found for feature matching.
Additionally, narrow streets are advantageous because the buildings are closer to the camera and can be captured in more detail. 
We are convinced our image and model-based navigation algorithm can inspire upcoming work. 
In the future, we plan to improve upon measurements, e.g., by using SLAM. With this, we hope to decrease the large deviations in our calculations from the GNSS points. Additionally, we will work on reducing false feature matches to achieve higher accuracy. 
We also plan to do more experiments on more datasets and aim to expand the LoD3 datasets.



§ ACKNOWLEDGMENT


The authors would like to thank 3D Mapping Solutions GmbH for providing the images to develop and test the algorithm.
This work was supported by the Bavarian State Ministry for Economic Affairs, Regional Development and Energy within the framework of the IuK Bayern project MoFa3D - Mobile Erfassung von Fassaden mittels 3D Punktwolken, Grant No. IUK643/001.
Moreover, the work was conducted within the framework of the Leonhard Obermeyer Center at the Technical University of Munich (TUM).
We gratefully acknowledge the tum2twin team at the TUM for the valuable insights and for providing the CityGML datasets.









	1.17
		 
	

















































































 Paper Title: DriveArena: A Closed-loop Generative Simulation Platform for Autonomous Driving.























-1.0ex
    < g r a p h i c s >
 : A Closed-loop Generative Simulation Platform for Autonomous Driving
    Xuemeng Yang^1, *   Licheng Wen^1, *    Yukai Ma^2, 1, *   Jianbiao Mei^2, 1, *   Xin Li^3,5, *, † 

 
 
  Tiantian Wei^1, 4, *   Wenjie Lei^2, †   Daocheng Fu^1   Pinlong Cai^1   Min Dou^1

 

  Botian Shi^1,   Liang He^5   Yong Liu^2,   Yu Qiao^1 




^1 Shanghai Artificial Intelligence Laboratory   ^2 Zhejiang University   ^3 Shanghai Jiao Tong University 




^4 Technical University of Munich   ^5 East China Normal University




Project Page: <https://pjlab-adg.github.io/DriveArena/>

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

 
 
 
* Equal contribution,    Corresponding author 

† Work performed during internships at Shanghai AI Laboratory



§ ABSTRACT 
This paper presented , the first high-fidelity closed-loop simulation system designed for driving agents navigating in real scenarios.  features a flexible, modular architecture, allowing for the seamless interchange of its core components: , a traffic simulator capable of generating realistic traffic flow on any worldwide street map, and , a high-fidelity conditional generative model with infinite autoregression. This powerful synergy empowers any driving agent capable of processing real-world images to navigate in 's simulated environment. The agent perceives its surroundings through images generated by  and output trajectories. These trajectories are fed into , achieving realistic interactions with other vehicles and producing a new scene layout. Finally, the latest scene layout is relayed back into , perpetuating the simulation cycle. This iterative process fosters closed-loop exploration within a highly realistic environment, providing a valuable platform for developing and evaluating driving agents across diverse and challenging scenarios.  signifies a substantial leap forward in leveraging generative image data for the driving simulation platform, opening insights for closed-loop autonomous driving.

Code will be available soon on GitHub: <https://github.com/PJLab-ADG/DriveArena>







§ INTRODUCTION



Autonomous driving (AD) algorithms have advanced rapidly in recent decades <cit.>, progressing from modular pipelines <cit.> to end-to-end models <cit.> and knowledge-driven methods <cit.>. Despite demonstrating outstanding performance across various benchmarks, significant challenges persist in evaluating these algorithms on replayed open-loop datasets, obscuring their real-world efficacy.
Public datasets <cit.>, while offering realistic driving data with authentic sensor inputs and traffic behavior, are inherently biased towards simple straight-ahead scenarios. In such cases, an agent can achieve seemingly good performance by merely maintaining its current state, complicating the assessment of actual driving capabilities in complex situations. 
Furthermore, the agent's current decision does not affect execution or subsequent decisions in the open-loop evaluation, which prevents it from reflecting cumulative errors in real-world driving scenarios.
Additionally, the static nature of recorded datasets, where other vehicles cannot react to the ego vehicle's behavior, further hinders the evaluation of AD algorithms in dynamic, real-world conditions.

As illustrated in Figure <ref>, we analyze existing AD methods and platforms, revealing that most of them are inadequate for a high-fidelity closed-loop simulation.
Ideally, as an aspect of embodied intelligence, agents should be evaluated in a closed-loop environment, where other agents react to the actions of the ego vehicle, and the ego vehicle receives changed sensor input accordingly. However, existing simulation environments either cannot simulate sensor inputs <cit.> or have a significant domain gap with the real world <cit.>, making it difficult to seamlessly integrate algorithms into the real world, thus posing a huge challenge for closed-loop evaluation. 
We believe that the simulator should not only closely reflect the visual and physical aspects of the real world, but also promote the continuous learning and evolution of the model within an exploratory closed-loop system for adapting to diverse complex driving scenarios. To achieve this goal, it is imperative to establish a high-fidelity simulator that complies with physical laws and supports interactive functionalities. 

Therefore, we present , a pioneering closed-loop simulator based on conditional generative models for training and testing driving agents. 
Specifically,  offers a flexible platform that can be integrated with any camera-input driving agent. It adopts a modular design and naturally supports iterative upgrades of each module.  consists of a  that manages traffic flow and a  based on auto-regressive generation.
 can generate realistic interactive traffic flow on any road network worldwide, while  is a high-fidelity conditional generative model with infinite autoregression. The driving agent should make corresponding driving actions based on the images generated by , and feed them back to  to update the status of vehicles in the environment. The new scene layout will be returned to  for a new round of simulation. This iterative process realizes the dynamic interaction between the driving agent and the simulation environment. The specific contributions are as follows:

 
    
  * High-fidelity Closed-loop Simulation: We propose the first high-fidelity closed-loop simulator for autonomous driving,  , which can provide realistic surround images and integrate seamlessly with existing vision-based driving agents. It can closely reflect the visual and physical properties of the real world, enabling agents to continuously learn and evolve in a closed-loop manner and adapt to various complex driving scenarios.
    
    
  * Controllability and Scalability: 
    Our  can dynamically control the movement of all vehicles in the scenarios and feed the road and vehicle layouts into , which utilizes a conditional diffusion framework to generate realistic images in a stable and controllable manner. Additionally,  supports simulation using road networks from any city worldwide, enabling the creation of diverse driving scenario images with varying styles. 
    
    
  * Modularized Design: The Driving Agent,  and  communicate via network interfaces, enabling a highly flexible and modular framework. This architecture allows each component to be replaced with different methods without requiring specific implementations. Functioning as an arena for these players,  facilitates comprehensive testing and improvement of both vision-based autonomous driving algorithms and driving scene generative models.






§  FRAMEWORK


As illustrated in Figure <ref>, the framework of our proposed  comprises two key components: a  functioning as the backend physical engine and a  serving as the real-world image renderer. Unlike conventional approaches,  does not rely on pre-built digital assets or reconstructed 3D road models. Instead, the  adapts to road networks of any city in OpenStreetMap (OSM) format <cit.>, which can be directly downloaded from the Internet. This flexibility enables closed-loop traffic simulations on diverse urban layouts.

The  receives ego trajectories output by the autonomous driving agent and manages the movement of all background vehicles. Unlike world model approaches <cit.> that rely on diffusion models for both image generation and vehicle movement prediction, our  utilizes explicit traffic flow generation algorithms <cit.>. This approach enables the generation of a wider range of uncommon and potentially unsafe traffic scenarios, while also facilitating real-time collision detection between vehicles.

 generates realistic camera images that precisely correspond to the 's output. It also allows for user-defined prompts to control various elements of the generated images, such as street view style, time of day, and weather conditions, enhancing the diversity of the generated scenes. Specifically, it employs a diffusion-based model that utilizes the current map and vehicle layouts as control conditions to produce surround-view images. These images serve as input for end-to-end driving agents. Given 's closed-loop architecture, the diffusion model is required to maintain both cross-view and temporal consistency in the generated images.

The generated multi-view images of the current frame are fed into the end-to-end autonomous driving agents, which can output the ego vehicle's movement. The planned ego trajectory is subsequently sent to  for the next simulation step. The simulation concludes when the ego vehicle either successfully completes the entire route, crashes, or deviates from the road. Upon completion,  performs a comprehensive evaluation process to assess the driving agent's capabilities.

It is noteworthy that  employs a distributed modular design. The , , and AD agent communicate via network using standardized interfaces. Consequently,  does not mandate specific implementations for the  or the AD agent. Our framework aims to function as an “arena” for these “players”, facilitating comprehensive testing and improvement of both end-to-end autonomous driving algorithms and realistic driving scene generative models.



§ METHODOLOGY



Following the  framework outlined above, we have implemented a preliminary version of . In this section, we elaborate on the implementation of each module: , , and AD agent, while describing necessary details that were not previously mentioned. At the end of this section, we present both the open-loop and closed-loop evaluation metrics for AD agents in .



 §.§ 

Most existing realistic driving simulators <cit.> rely on limited layouts from public datasets, lacking diversity for dynamic environments. To address these challenges, we utilize LimSim <cit.> as the underlying  to simulate dynamic traffic scenarios and generate road and vehicle layouts for subsequent environment generation. LimSim also provides a user-friendly front-end GUI, which directly displays the BEV map and results from  and the driving agent.

Our  enables interactive simulations of multiple vehicles in traffic flow, including comprehensive vehicle planning and control. We adopt a hierarchical multi-vehicle decision-making and planning framework, which jointly makes decisions for all vehicles within the flow and reacts promptly to the dynamic environment through a high-frequency planning module <cit.>. The framework also incorporates a cooperation factor and trajectory weight set, introducing diversity to autonomous vehicles in traffic at both social and individual levels.

Furthermore, our dynamic simulator supports various custom HD maps of any city from OpenStreetMap, facilitating the construction of diverse road graphs for convenient simulation. 
The  controls the movement of all background vehicles. For the ego vehicle, we provide two distinct simulation modes: open-loop and closed-loop. In closed-loop mode, the driving agent performs planning for the ego vehicle, and  uses the agent-outputted trajectory to control the ego vehicle accordingly. In open-loop mode, the trajectory generated by the driving agent is not actually used to control the ego vehicle; instead,  maintains control in a closed-loop manner. The details of these two modes are further elaborated in Section <ref>.



 §.§ 



Unlike recent autonomous driving generation methods <cit.> that use Neural Radiance Fields (NeRF) and 3D Gaussian Splatting for environment reconstruction from logged video, we design a diffusion-based . It utilizes control conditions of the map and vehicle layouts from the  to generate geometrically and contextually accurate driving scenarios. Our framework shares several advantages: (1) Better controllability. The generated scenes can be controlled by scene layouts from , textual prompts, and reference images to capture different weather conditions, lighting, and scene styles. (2) Better scalability. Our framework can adapt to various road structures without the need to model the scene in advance. In theory, we support the generation of driving scenes for any city in the world by leveraging layouts from OpenStreetMap.

We illustrate our diffusion-based  in Figure <ref>. Built upon the stable diffusion pipeline <cit.>,  utilizes an effective condition encoding module that accepts a variety of conditional inputs including map and vehicle layouts, text descriptions, camera parameters, ego poses, and reference images to generate realistic surround-view images. Considering the importance of ensuring synthesis scene consistency across different views and time spans for driving agents, we integrate a cross-view attention module, inspired by <cit.>, to maintain coherence across different views. Additionally, we adopt an image auto-regressive generation paradigm to enforce temporal consistency. This approach enables  to not only maximally maintain the temporal consistency of the generated videos, but also generate videos of arbitrary length in an infinite stream, which provides great support for autonomous driving simulation.

Condition encoding. Previous work <cit.> applied BEV layout as conditional input to control the output of the diffusion model, which increased the difficulty of the network in learning to generate geometrically and contextually accurate driving scenes. In this work, we present a new condition encoding module to introduce more guidance information, which helps the diffusion module generate high-fidelity surround images. Specifically, in addition to encoding camera poses for each view, text descriptions, 3D object bounding boxes, and BEV map layouts using a condition encoder similar to <cit.>, we also explicitly project the map and object layouts onto each camera view to generate layout canvases for more accurate lane and vehicle generation guidance. 
Specifically, the text embedding e_text is obtained by encoding the text descriptions with the CLIP text encoder <cit.>. The parameters 𝐏 = {𝐊∈ℝ^3 × 3, 𝐑∈ℝ^3 × 3, 𝐓∈ℝ^3 × 1} of each camera and the 8 vertices of the 3D bounding boxes are encoded to e_cam and e_box by Fourier embedding <cit.>, where 𝐊, 𝐑, 𝐓 represent camera intrinsic, rotations and translations respectively. The 2D BEV map grid uses the same encoding method as in  <cit.> to get embedding e_map. Then, each category of the HD maps and the 3D boxes is projected onto the image plane to obtain the map canvas and box canvas, respectively. These canvases are concatenated to create the layout canvas. The final feature e_layout can be obtained by encoding the layout canvas by the conditional encoding network <cit.>.

Moreover, we introduce a reference condition to provide appearance and temporal consistency guidance. During training, we randomly extract a frame from the past 𝐋 frames as a reference frame and use the pre-trained CLIP model <cit.> to extract reference features e_ref from the multi-view images. The encoded reference features imply semantic context and are integrated into the conditional encoder through the cross-attention module. In order to make the diffusion model aware of the motion changes of the ego-vehicle, we also encode the ego-pose relative to the reference frame into the conditional encoder to capture the motion change trend of the background. The relative pose embedding e_rel is encoded by Fourier embedding.
By incorporating the above control conditions, we can effectively control the generation of surround images.

Auto-regressive generation. To facilitate online inference and streaming video generation while maintaining temporal coherence, we have developed an auto-regressive generation pipeline. Specifically, during the inference phase, the previously generated images and the corresponding relative ego pose are used as reference conditions. This approach guides the diffusion model to generate current surround images with enhanced consistency, ensuring a smoother transition and coherence with the previously generated frames.

What we designed in this paper is just a simple implementation of the . We also verify that extending the auto-regressive generation to a multi-frame version (using multiple past frames as references and outputting multi-frame images) and adding additional temporal modules can improve temporal consistency.



 §.§ Driving Agent

Recent works <cit.> have demonstrated the challenges in justifying the planning behavior of driving agents through open-loop evaluation on public datasets <cit.>, primarily due to the simplistic nature of driving scenarios presented. While some studies <cit.> have conducted closed-loop evaluations using simulators like CARLA <cit.>, discrepancies such as appearance and scene diversity persist between these simulations and the dynamic real world. To bridge this gap, our  provides a realistic simulation platform with the corresponding interfaces for camera-based driving agents <cit.> to perform more comprehensive evaluations, including both open-loop and closed-loop testing. Moreover, by changing the input conditions, such as the road and vehicle layouts,  could generate corner cases and facilitate these driving agents' evaluation on out-of-distribution scenarios. Without loss of generality, we select a representative end-to-end driving agent, namely UniAD <cit.>, to conduct both open-loop and closed-loop testing in our . UniAD utilizes surround images to predict motion trajectories for the ego vehicle and other agent vehicles, which can be seamlessly integrated with the API of our dynamic simulator for evaluation. Furthermore, the perceptual outputs, such as 3D detection and map segmentation, contribute to enhancing the validation of realism in our environment generation.





 §.§ Ego Control Modes and Evaluation Metrics



 inherently supports “closed-loop” simulation mode of driving agents. That is, the system adopts the trajectory output by the agent at each timestep, updates the ego vehicle's state based on this trajectory, and simulates the actions of background vehicles. Subsequently, it generates multi-view images for the next timestep, thus maintaining a continuous feedback closed-loop.
Additionally, recognizing that some AD agents may be unable to perform long-term closed-loop simulation during the development process,  also supports the “open-loop” simulation mode. In this mode, the  will take over the control of the ego vehicle, while the trajectory output by the AD agent is recorded for subsequent evaluation.

In both open-loop and closed-loop modes, it is crucial to comprehensively evaluate AD agent performance from a results-oriented perspective. Drawing inspiration from NAVSIM <cit.> and the CARLA Autonomous Driving Leaderboard <cit.>,  adopts two evaluation metrics: PDM Score (PDMS) and Arena Driving Score (ADS).


PDMS, initially proposed by NAVSIM <cit.>, evaluates the trajectory output at each timestep. We adhere to the original definition of PDMS, which aggregates the following sub-scores:

    PDMS_t=   (∏_m ∈{NC, DAC} score _m)_penalties×
       (∑_w ∈{EP, TTC, C}weight_w  × score _w/∑_w ∈{EP, TTC, C}weight_w)_weighted average .

where the penalties include the drive with no collisions (NC) with road users and drivable area compliance (DAC), as well as the weighted average including ego progress (EP), time-to-collision (TTC), and comfort (C). 
We implement minor modifications tailored to : in score _NC, we do not differentiate “at-fault" collisions, and for score _EP, we utilize the 's Ego path planner as the reference trajectory instead of the Predictive Driver Model.
At the end of the simulation, the final PDM Score is averaged across all simulation frames.

    PDMS = Σ_t=0^TPDMS_t/T∈ [0,1]


For open-loop simulations, PDMS serves directly as the evaluation metric for AD agents. However, for driving agents operating under the “closed-loop" simulation mode, we employ a more comprehensive metric called Arena Driving Score (ADS), which combines the trajectory PDMS with route completion:

    ADS = R_c×PDMS

where R_c∈ [0,1] represents route completion, defined as the percentage of the route distance completed by an agent. Given that “closed-loop" simulations terminate upon agent collision with other road users or deviation from the road, ADS provides a suitable metric for differentiating agents' driving safety and consistency.




§ EXPERIMENTS

 



 §.§  Setups

Dataset. 
For , we use the nuScenes <cit.> dataset for training. Following the official configuration, we employ 700 scenes for training and 150 for validation. We focus on four road categories (lane boundary, lane divider, pedestrian crossing, and drivable area) and ten object categories. The nuScenes dataset contains data collected from four different cities, covering various light and weather conditions, including daytime, night, sunny, cloudy, and rainy scenarios, enabling  to conditionally imitate diverse appearances. We additionally annotated each scene using GPT-4V, providing detailed scene descriptions that include elements like time, weather, street style, road structure, and appearance. These descriptions serve as text prompt conditions.






Model Setup. 
The model is initialized with the pre-trained Stable Diffusion v1.5 <cit.>, with only the newly added parameters being trained. For various conditions, except for the encoding of reference images and text prompts, the encoders for other conditions are randomly initialized and trained from scratch. These conditions are then integrated into the UNet using a randomly initialized ControlNet <cit.> to control the denoising process.

Training and Inference. 
To utilize the reference images and achieve temporal correlation, we employ ASAP <cit.> to generate 12Hz interpolated annotations and crop them into image clips of length 𝐋=7. During training, we use the last frame of each clip as the current frame, select any frame from the clip as the reference frame, and calculate the relative pose between them to model the motion trend of the background. Accordingly, the surround images corresponding to the reference frame are input to the network as reference images. During inference, the generated result of the previous frame is used as the current reference images, enabling unlimited length generation. 
The experiment is conducted on 8 NVIDIA A100 (80GB) GPUs with a batch size of 4×8 and 200K iterations of training. The AdamW optimizer is used with a learning rate of 1e-4.
The network follows the same image resolution (224×400) as MagicDrive, and when input to the driving agent, it will be upsampled to the original image size of nuScenes (900×1600) through a super-resolution algorithm <cit.>.



 §.§  Setups


Operating Frequencies.
In our experiments, the  operates at a frequency of 10Hz, while the control frequency is set to 2Hz. This configuration results in the  sending the current layout to  every 0.5 simulation seconds, requesting surround images. These images are then forwarded to the driving agent, which predicts and plans the subsequent trajectory for the ego vehicle. The , , and driving agent communicate via HTTP protocol, enabling deployment across different servers.

Simulation Modes.
As detailed in Section <ref>, we implement two simulation modes. In the open-loop mode, all vehicles, including the ego vehicle, are controlled by  itself. The driving agent can predict the ego vehicle's trajectory, but its trajectory is not actually executed. In the closed-loop mode, the ego vehicle is controlled by the driving agent, and the simulation terminates if it crashes with other vehicles or leaves the road.


Supported Maps.
Currently,  supports four different maps, which are: , , , and . The first two maps closely resemble the corresponding areas in the nuScenes dataset, while the last one replicates the road network of the Town05 map in the CARLA simulator. Notably,  can download road network data for any area directly from OpenStreetMap and perform simulations, enabling  to simulate the road network of almost any city worldwide. OpenstreetMap also accepts customized operations, and users can draw the desired road network structure for simulation testing.




 §.§  Fidelity Validation

To assess the sim-to-real gap between our generated images and the original nuScenes images, we employ UniAD <cit.> as an evaluator. We generate videos for 150 scenes based on the original layout provided by the nuScenes validation set with 2Hz. For comparative analysis, we set MagicDrive as the baseline method and use its official codes and checkpoints for inference. Subsequently, UniAD is performed on these images to compute various metrics, including 3d object detection, BEV map segmentation, and planning. The results are summarized in Table <ref>. It shows that all our indicators are higher than the baseline method, and a few indicators even surpass the performance on the original nuScenes. Furthermore, it demonstrates our model's superior capability to accurately respond to control signals and strictly adhere to input conditions. These findings establish a solid foundation for using our generator as a reliable simulator.









 §.§ Visualization

Controllability.
In this section, we will comprehensively demonstrate the controllability of the model from various dimensions, including the control of lighting and weather, the fit of object boxes and maps, change of street style, and consistency over long periods of time.

We demonstrate the impact of the reference image on the generated image, as shown in Figure <ref>. We randomly select one frame of images from the nuScenes dataset as reference images and choose three scenes from OpenStreetMap and Carla. We perform inference on them with  respectively. It can be seen that the source and style of the road network are very different from the scope of the original nuScenes dataset. The pictures show that the generated vehicles and road networks conform closely to control conditions, demonstrating strong control capabilities. The style and weather of the generated pictures can also be consistent with the reference images. In other words, besides maintaining image generation continuity through reference images, we can also regulate image style accordingly.

Figure <ref> presents images generated using different text prompts and reference images on the same road network. Each set of images portrays the surrounding scenery at intervals of 8.5 seconds and 24 seconds respectively, with the layout projected on the image. The images clearly 
illustrate that the road structure and vehicles strictly adhere to the given control conditions while maintaining excellent consistency in the surround view. In addition, the four sets of images exhibit significant differences in weather and lighting and can maintain their own styles during the continuous iteration process.


Scalability.
The  can accept any map downloaded from OpenStreetMap and seamlessly connect to the Carla road network. Combined with Dreamer's excellent following capability, the entire framework demonstrates robust scalability. The specific results are shown in Figure <ref>. We used both MagicDrive and our  to generate realistic images on the same Carla road network, with the corresponding lane lines projected onto the images. The road style in Carla differs significantly from that of nuScenes. It is rare to encounter roads with such large curvature and such wide roads in nuScenes. Consequently, the performance of MagicDrive, which is based on the nuScenes BEV map, is slightly inferior in these conditions. As indicated by the yellow arrow, MagicDrive struggles to generate curved roads and fit wide roads accurately. , however, can produce reasonable pictures that follow the road structure.


We also demonstrate additional cases using data from the nuPlan dataset to validate the scalability. The nuPlan data originates from cities different from nuScenes and features varying camera numbers and parameters. We select 6 cameras with a similar layout to the nuScenes dataset, and nuPlan's camera parameters are employed to project object boxes and lane lines onto corresponding images as control conditions. As shown in Figure <ref>,  fully trained on nuScenes adeptly adheres to these conditions, generating coherent images when deployed in new cities and even with novel camera configurations.


















 §.§ Open-loop and Closed-loop Experiments


In this section, we adopt the prevailing end-to-end autonomous driving method UniAD <cit.> as the driving agent to test both the open-loop and closed-loop performance within the  framework. We utilized UniAD's open-source code and pre-trained weights without additional training. UniAD operates at 2Hz, outputting a trajectory of 6 path points over the next 3 seconds.  further interpolates this to a 10Hz trajectory.

Open-loop Evaluation.
We first assess UniAD's performance in 's open-loop mode.
UniAD is evaluated on three scenarios: 1) the original nuScenes image sequences; 2) -generated nuScenes image sequences, where the vehicles' trajectory remains identical to nuScenes ground truth, but surround images are replaced with -generated ones; and 3) 's own simulation sequences (i.e., 's open-loop mode).
Our evaluation metrics consist of the PDM Scores and its sub-scores, as detailed in Section <ref>. Additionally, we evaluate trajectories driven by human drivers in nuScenes as the human driver performance. Detailed results are presented in Table <ref>.

The results reveal that while UniAD performs optimally on the original nuScenes sequence with a PDMS metric of 0.91, the -generated sequence surprisingly achieves a PDMS of 0.902, representing a metric drop of less than 1%. We attribute this to both the high fidelity of our -generated images and UniAD's strong dependence on ego states, as corroborated by <cit.>.

In 's open-loop mode, Figure <ref> illustrates two sequences, demonstrating that UniAD's prediction of the road network and vehicle tracking are fundamentally accurate. However, in terms of metrics, UniAD's performance in such scenarios with unseen road and traffic flow is significantly degraded, with an average PDM Score of only 0.636. The output trajectories by UniAD exhibit a substantial increase in collision rates and instances of driving outside the drivable area.
The open-loop experimental results underscore the critical importance of closed-loop experiments and tests for autonomous driving methods.

Closed-loop Evaluation.
We further evaluated UniAD's performance in 's closed-loop mode. In this mode, the trajectory outputted by UniAD is directly used for ego vehicle control, and the evaluation metrics include PDM Score (PDMS), Route Completion (RC), and Arena Drive Score (ADS). Our closed-loop experiment was conducted on four pre-set paths, with two paths selected in Boston and two in Singapore. The simulation time to complete each trajectory was approximately 120 seconds. Detailed results are presented in Table <ref>.

The results indicate that the PDMS of UniAD-generated trajectories in closed-loop mode (0.667) is comparable to that of the open-loop mode. However, the Route Completions (RC) are consistently low, averaging only about 13.7% of the total route length. Specifically, UniAD performs better on straightaways but largely fails to navigate the first turning intersection in the route. Figure <ref> illustrates two failure cases where UniAD lacked sufficient trajectory correction. Despite a roughly correct prediction of the road structure, it ultimately mounted the central green belt or failed to complete a right turn successfully. The average Arena Driving Score for UniAD is 0.086.
It should be noted that these are preliminary results based on testing only 4 routes. We plan to expand the number of routes for a more comprehensive evaluation and explore the combined effect of 's timing consistency and the driver agent's performance on the final ADS.




§ RELATED WORKS





 §.§ Data Acquisition for Autonomous driving


The characteristics of the automated driving dataset can be categorized into two aspects: appearance fidelity and interactivity. First, in terms of appearance fidelity, NGSIM <cit.> and CitySim <cit.> provide only realistic driving trajectories that can provide safe and reliable driving planning guidance. On top of that, some datasets developed based on the Carla simulator, such as DriveLM-CARLA <cit.> and BenchDrive <cit.>, provide simulated sensor data. Taking it a step further, the Waymo <cit.> and nuScenes <cit.> datasets capture real-world sensor recordings and the driving behavior of human drivers. The datasets were produced in a complex process and with a limited amount of data. To add variety to the scenarios, MagicDrive <cit.> and DriveDreamer <cit.> provide editable scenario generation. So far, we have been able to obtain diverse and rich data for training. However, the above data can only be used for open-loop evaluation, i.e., current decisions do not affect future data distributions, which differs significantly from real driving. Works <cit.> that also have fidelity differences, improve the interactivity of the data, they usually use auto-regressive generation methods to realize the interaction, the generation process implies the model's understanding of the world, and usually can not be too much human intervention. Some simulators <cit.> make things more controllable by decoupling part of the mechanics of how the world works. Common examples include simulators <cit.> that provide realistic traffic flow, and simulators <cit.> that drive vehicles in game engines, and reconstructive simulations represented by <cit.> that provide the appearance of reality.




 §.§ Diffusion-based Generative Models

Recent advancements in generative models have seen diffusion models play a pivotal role in image and video generation <cit.>. Moreover, recent works have expanded the scope by integrating additional control signals beyond traditional text prompts <cit.>. For instance, ControlNet <cit.> incorporates a trainable version of the SD encoder for control signals, while studies such as Uni-ControlNet <cit.> and UniControl <cit.> have emphasized the fusion of multi-modal inputs into a unified control condition using input-level adapter structures. 
Our approach aims to study the generation of continuous and controllable sequence frames, thereby bridging the gap between simulation environments and reality, and establishing the required foundation for closed-loop learning of autonomous driving agents.




 §.§ Evolution of Autonomous Driving Generation
 

World Models <cit.> utilize diffusion models to generate future driving scenes based on historical information, these methods often lack the ability to control the scenarios through layout, are difficult to generate continuous and stable videos and lack the approximation of physical laws.
TrackDiffusion focused on generating videos based on 2D object layouts <cit.>.
BEVGen <cit.> pioneered the generation of synthetic multi-view images based on the BEV layout, laying the foundation for a controllable generation of autonomous driving scenarios. BEVControl <cit.> extended this approach by a height elevation process, enabling image generation aligned with surrounding projection layouts.
Further advancements includes MagicDrive <cit.>, DriveDreamer <cit.>, Panacea <cit.> and DrivingDiffusion <cit.>, which generate panoramic controllable videos through various 3D controls and encoding strategies.
However, their primary focus lies in augmenting training data to enhance algorithm performance, rather than serving as simulators for modeling dynamic environmental interactions.



 §.§ Simulator-Driven Scenario Generation

Autonomous vehicle development is significantly enhanced by driving simulators, which provide controlled environments for realistic simulation. Prominent research efforts have concentrated on generating virtual imagery and annotations, with some studies expanding to incorporate environmental variations and construct safety-critical scenarios for training based on real-world data logs. Nevertheless, these simulated images frequently fall short of achieving true realism, as evidenced by previous works <cit.>. 
While SimGen <cit.> made a breakthrough as the first work to generate diverse driving scenarios following conditions from a simulated environment, it mainly focused on the quality of the generated content with only front-view images, neglecting the exploration of closed-loop systems. Our research aims to bridge this gap by developing a system that can not only generate realistic scenarios but also allow agents to interact with them in a closed-loop manner.




 §.§ Closed-Loop Driving in Simulation

End-to-end vehicle control algorithms <cit.>, are typically trained and evaluated on open-loop datasets <cit.>. However, these algorithms lack the capability to generalize directly to simulators for closed-loop evaluation, which hinders the demonstration of their true performance potential. Recent studies have increasingly recognized the significance of closed-loop evaluation, as exemplified by <cit.>. Moreover, simulation environments offer a wealth of training data, a stark contrast to models trained on datasets that are constrained by data distribution <cit.>. A significant challenge arises due to the discrepancy between the simulated scene's appearance and real-world conditions, complicating the generalization of models trained on simulation data to actual scenarios. This creates a paradox: the desire to utilize simulation data for its diversity and editability, while also seeking data that closely mirrors reality. Our approach effectively addresses this issue by enhancing the realism of the simulator for certain closed-loop learning methods <cit.>.




§ CONCLUSIONS AND FUTURE WORKS


This paper introduces a novel closed-loop simulation platform named  for vision-based driving agents.  integrates a  that generates human-like traffic flow and a high-fidelity generative  with infinite generation. This combination allows realistic interaction and continuous feedback between the driving agent and the simulation environment. The system provides a valuable platform for developing and testing autonomous driving agents in a variety of scenarios, marking a substantial leap in driving simulation technology.


 is designed with a modular architecture, allowing for easy replacement of each module. This paper presents an initial implementation of . As the first high-fidelity closed-loop simulator, we still have a few limitations for future improvement:

1) Data Diversity: The current generative model is trained solely on the nuScenes dataset, which limits the diversity and emergence capabilities. We plan to expand training to include more varied datasets to enhance the model's robustness and versatility.

2) Temporal Consistency: While we can generate continuous videos with an autoregression strategy, maintaining motion trends and temporal consistency between frames remains challenging. Future work will explore multi-frame autoregressive networks and more scalable architectures <cit.> to address these issues.

3) Runtime Efficiency: Like many generative models,  requires significant runtime. Investigating faster sampling methods <cit.> and model quantization may alleviate these problems.

4) Expanded Agent Testing: We plan to incorporate a broader range of driving agents within , facilitating the continuous learning and evolution of knowledge-driven driving agents in the closed-loop environment <cit.>.

5) A Real Arena:  can not only evaluate the performance of different driving agents, but also act as a testing ground for AD generative models. By using the same driving agent as a referee, it can fairly assess the sim-to-real gap of different generative models. This approach even provides a more credible and convincing evaluation compared to traditional metrics like FID and FVD.

We recognize that practical application may still be a way off, but the potential and promise shown by this work are evident. We hope this research will advance closed-loop exploration in highly realistic environments and offer a valuable platform for developing and assessing driving agents across a range of challenging scenarios. We encourage the community to collaborate in advancing this field. The era of open loops is transitioning, and autonomous driving evaluation and learning are set to enter a new era of closed-loop systems.



§ ACKNOWLEDGMENTS

The research was supported by Shanghai Artificial Intelligence Laboratory, the National Key R&D Program of China (Grant No. 2022ZD0160104) and the Science and Technology Commission of Shanghai Municipality (Grant Nos. 22DZ1100102 and 23YF1462900).




ieeetr





 Paper Title: Analysis of Functional Insufficiencies and Triggering Conditions to Improve the SOTIF of an MPC-based Trajectory Planner.
















Analysis of Functional Insufficiencies and Triggering Conditions to Improve the SOTIF of an MPC-based Trajectory Planner
    Mirko Conrad[samoconsult GmbH, Französische Str. 13, 10117 Berlin, Germany], Georg Schildbach[University of Lübeck, Ratzeburger Allee 160, 23562 Lübeck, Germany corresponding author ()]
    July 2024
=============================================================================================================================================================================================





§ ABSTRACT

Automated and autonomous driving has made a significant technological leap over the past decade. In this process, the complexity of algorithms used for vehicle control has grown significantly. Model Predictive Control (MPC) is a prominent example, which has gained enormous popularity and is now widely used for vehicle motion planning and control. 
However, safety concerns constrain its practical application, especially since traditional procedures of functional safety (FS), with its universal standard ISO 26262, reach their limits. Concomitantly, the new aspect of safety-of-the-intended-function (SOTIF) has moved into the center of attention, whose standard, ISO 21448, has only been released in 2022.  Thus, experience with SOTIF is low and few case studies are available in industry and research. 
Hence this paper aims to make two main contributions: (1) an analysis of the SOTIF for a generic MPC-based trajectory planner and (2) an interpretation and concrete application of the generic procedures described in ISO 21448 for determining functional insufficiencies (FIs) and triggering conditions (TCs). Particular novelties of the paper include an approach for the out-of-context development of SOTIF-related elements (SOTIF-EooC), a compilation of important FIs and TCs for a MPC-based trajectory planner, and an optimized safety concept based on the identified FIs and TCs for the MPC-based trajectory planner.



§ KEYWORDS

Automated Driving System (ADS) ,  
Autonomous Vehicles ,  
Trajectory Planning ,  
Model Predictive Control (MPC) ,  
Functional Safety (FS) ,  
Safety of the Intended Functionality (SOTIF) ,  
Functional Insufficiencies (FIs) ,  
Triggering Conditions (TCs) ,  
SOTIF-related Element out of Context (SOTIF-EooC)




§ INTRODUCTION


Since the beginning of the 21st century, automated and autonomous vehicles have become a major subject of research across the globe, in academia and industry <cit.>. The technological progress is fueled by ever more powerful hardware for computation, communication, and sensing. Another relevant factor is the rapid advances in mathematical algorithms, especially in Artificial Intelligence (AI), Machine Learning (ML), and Numerical Optimization (NO). A plethora of new and powerful methods have been developed and successfully demonstrated in actual vehicles on the road, among them Model Predictive Control (MPC). 

MPC is a popular computer-based control method <cit.> that has been frequently tested in automated driving system (ADS) prototypes <cit.>. Thus far, however, it has not been considered a viable option for series production. The main reason is its relatively high complexity, causing relatively high efforts in the development process and increasing the computational hardware requirements. Arguably, an even bigger challenge results from safety concerns related to MPC. These concerns are similar to other state-of-the-art methods of AI, ML, and NO. 

Due to the complexity of these methods, and the complexity of the corresponding automated driving tasks, Functional Safety (FS), which is concerned with safety in the presence of malfunctions, is no longer the single most important aspect of safety. Instead, Safety of the Intended Function (SOTIF), which is concerned with the  safety of the nominal function even if all system elements work as intended, becomes equally important. The intention of this paper is hence to present a contribution towards a viable SOTIF concept for an MPC-based motion planning module in automated driving systems (ADSs).



 §.§ Model Predictive Control


Over the past decades, MPC has developed into a mainstream approach for the control of complex and safety-critical systems, including ADSs. The basic idea is to formulate and solve a finite-horizon optimal control problem (FHOCP) as a numerical optimization program in real time <cit.>. The FHOCP contains a model of the system and naturally integrates constraints on the model states and control inputs. It can be used for motion planning <cit.> and trajectory tracking control <cit.>, where feedback is introduced by a receding-horizon implementation. It can also be used as a hybrid module, covering some or all aspects of planning and tracking control <cit.>. It has even been used to cover some aspects of tactical planning, such as lane change decisions <cit.>.

The current trend is to use MPC as a motion planner, such as in the commercial product ProDriver by Embotech <cit.>. The goal is to integrate as many aspects as possible into the motion planning module, in order to slim down the module interfaces. Interfaces always require additional engineering efforts and they are a common source of functional, and thus also safety problems. The MPC module should thus be capable of making tactical decisions, by introducing integer variables, and ideally should also provide a basic level of vehicle and actuator control.

Recent research in the context of MPC for ADSs has begun to focus on interaction-aware motion planning <cit.>. The basic idea is to consider the predictions of dynamic objects in the environment not as invariably fixed, but as interdependent with the other objects and also dependent on the actions of the ego vehicle. Hence the future trajectories of dynamic objects are included in the model-based predictions in the MPC. The interactions themselves can be modeled by different approaches, such as game theory <cit.> or neural networks <cit.>. 

A common idea is to represent the intention of other drivers as modes <cit.>, corresponding to lane keeping or a lane change, for example. The remaining uncertainty regarding the exact trajectory of the dynamic object can be considered as stochastic noise. Another noteworthy approach is Branch MPC <cit.>, where the modes of each dynamic object and the ego vehicle may change at several specific instances over the prediction horizon.



 §.§ Safety of the Intended Function (SOTIF)


SOTIF is a comparatively new branch of safety that is becoming increasingly important for SAE driving automation levels 3 or higher. The first edition of the applicable SOTIF standard ISO 21448 has only been published in 2022 <cit.>. 
Due to its relatively recent introduction, scientific literature on the application of SOTIF to ADSs is still  sparse. 
Zhu et. al. <cit.> emphasize that the research on the SOTIF of automated and autonomous driving systems is still in its infancy. Their paper focuses on safety analysis methods that could be applied to SOTIF, but it does not cover specifically the topic of identifying and analyzing functional insufficiencies (FIs) or triggering conditions (TCs).


Qidong et. al. <cit.> state that the identification and systematic analysis of FIs and TCs remains a major challenge for the SOTIF. Using an Adaptive Cruise Control (ACC) system as an example, the authors propose a scenario-based analysis method of SOTIF triggering conditions. In this approach, the TCs are analyzed according to elements of SOTIF scenarios that are formed in view of FIs. 
The approach is then applied to the 'Sense' portion of the ACC system. The 'Plan' part is not addressed in this paper.



Junfeng et. al. <cit.> apply the System-Theoretic Process Analysis (SPTA) method to study the SOTIF of a Lane Keep Assistance (LKA) system. The study intends to verify the feasibility of STPA as part of the SOTIF lifecycle.
The paper proposes a set of TCs for the LKA, but not all of them seem to be TCs in the sense of ISO 21448. A subset of these TCs will be used as input for the SOTIF analysis of the MPC-TP. 
In the analysis, there is also no specific focus on the 'Plan' part.



 §.§ Content and Contributions


This paper describes a possible approach for the analysis of FIs and TCs, with the goal of improving the SOTIF of a generic planning module for an ADS. The trajectory planning module is based on the Model Predictive Control (MPC) approach. It is assumed that the MPC-based trajectory planner (MPC-TP) can be integrated into different vehicle-level functions and that it is re-usable for different vehicles within a specified vehicle class. Hence it is treated as a SOTIF-related element out of Context (SOTIF-EooC), which is a particular novelty compared to the existing literature.

The main objective of this paper is to demonstrate how the framework of ISO 21448 <cit.> can be used for improving the SOTIF of a modern, state-of-the-art planning algorithm. The system architecture and functional specifications are outlined in Section <ref> and the details of the MPC-TP are provided in Section <ref>. The relevant SOTIF background is outlined in Section <ref>. The main part of this paper is the SOTIF analysis in Section <ref>, which describes the application of various methods and techniques from ISO 21448 <cit.> towards the identification of FIs and TCs for the MPC-TP. The analysis leads to a refined specification and design of the MPC-TP, as described in Section <ref>. The conclusions of this work are presented in Section <ref>. The main contributions of this paper can be summarized as follows:


  * Application of the abstract / generic framework of the new SOTIF standard ISO 21448 to a realistic case study, with a focus on early parts of the SOTIF lifecycle;

  * Interpretation and application of generic methods in ISO 21448 for the determination of FIs / TCs, in the context of a specific use case;

  * Derivation of a structured list of FIs / TCs as a consolidated result of the SOTIF analysis (in a tabular format);

  * A refinement of the notion of an out-of-context development in the scope of SOTIF.










§ INITIAL CONCEPT FOR VEHICLE LEVEL CONTROL SYSTEM






The starting point for the design of the planning module as a SOTIF-SEooC are assumptions about the encompassing vehicle control system into which the module is to be embedded, as well as the related interfaces. Furthermore, specifications are needed about the overall functionality and the operational design domain (ODD). 



 §.§ Functional Architecture


The functional architecture of a generic ADS is  shown in Figure <ref>. The MPC-TP is shown in the middle (red). The task of the MPC-TP is to compute a reference trajectory, which is then used by the trajectory controller (blue) and lower-level modules to control the vehicle's actuators. The actuators are an integral part of the chassis, including, in particular, the drivetrain and braking systems for longitudinal motion control and the steering system for lateral motion control. 

The MPC-TP receives information from centralized estimation modules (green), which may serve also other functions and/or other modules in the architecture <cit.>. The key point is that they are developed independently of the MPC-TP. These modules provide a local map, including the topology and road geometry and information about the infrastructure, such as traffic signs (Map Fusion). They also include the relative position of the vehicle on the map, the vehicle's dynamic states, a list of relevant vehicle parameters, and diagnostic information about the vehicle's sensors and actuators (Ego Motion Fusion). Furthermore, a list of relevant traffic objects in the vicinity of the vehicle is provided, including their classification, position, speed, and other states, as well as a situation analysis and prediction(s) about (possible) future evolution of the situation (Object Fusion and Prediction). The sensors also deliver important information about the environment, such as ambient temperature or road surface (Environment Fusion). Finally, the MPC-TP may receive high-level commands by the human driver or the ADS, such as the reference speed, which lane or road to use, or where to take a turn (Tactical Planner).









	






 §.§ Encompassing Vehicle Control System


The MPC-TP is treated as a SOTIF-related element out of context (SOTIF-EooC), as detailed in Section <ref>. Hence it shall be possible to integrate the MPC-TP into different the SOTIF-related systems as well as different vehicle types. For this paper, it is assumed that the MPC-TP can be used in the context of the following systems for a given class of passenger cars:

    
  F1) Energy Efficient Lane Centering and Adaptive Cruise Control (E2LCACC): E2LCACC is a SAE level 2 function that combines the classic Lane Centering (LC) and Adaptive Cruise Control (ACC). It performs the lateral and longitudinal control of the vehicle, in order to keep the vehicle on a reference path in its lane and to follow the preceding vehicle. E2LCACC contains AI-based elements for energy efficient driving, in particular to determine the optimal speed profile. 
    
  F2) Active Lane Change Assist (ALCA): ALCA is SAE level 2 or 3 function designed to help drivers changing lanes safely by automatically steering the vehicle into the adjacent lane. ALCA operates in combination with E2LCACC and also provides longitudinal and lateral control of the vehicle during overtaking. The driver can issue lane change requests to the left or the right, e.g., by operating the indicator lever. The ALCA then checks whether a safe lane change is possible within 10 seconds. If it is possible, it performs the lane change; if it is not possible, it keeps the vehicle in the current lane and continues with E2LCACC.
    
  F3) Highway Pilot (HP): The HP is a SAE level 4 function that takes full control of the vehicle during highway driving. Only high level objectives are provided by the user, such as the destination, the desired route, the targeted arrival time, or the driving style. The driving itself, including all decisions, is performed autonomously by the HP.





 §.§ Operational Design Domain (ODD)


The vehicle-level functions F1, F2, F3 are only to be used within the conditions and limits of the ODD () specified in Table <ref>. The ODD documents the specific conditions under which the ADS is designed to function <cit.>. Its description is based on the structure provided in DOT HS 812 623 <cit.>.





 §.§ Assumptions about the Functionality and the Vehicle Class


Since the MPC-TP is considered as a SOTIF-EooC, specific assumptions are required about its integration into the vehicle and the vehicle-level functions F1, F2, F3, as well as its interfaces to other modules. An overview of the upstream and downstream modules can be gathered from Figure <ref>. The perception modules make use of appropriate sensors and communication infrastructure. The control modules are connected with the relevant vehicle actuators. It is beyond the scope of this paper to specify these modules here in detail. A list of the most relevant specifications for the ego vehicle (EgoV) and the module interfaces is provided below.


    
  Spec:ODDj) The EgoV belongs to a specific class of passenger vehicles, consisting of a finite number of vehicle types.
    
  Spec:ODDk) All possible EgoV types are known at the time of the MPC-TP development process, including their physical design and specifications, chassis and suspension systems, and chassis control systems (Anti-lock Braking System, Electronic Stability Control, etc.).
    
  Spec:ODDl) Further parameters of EgoV (such as the vehicle mass, location of the center of gravity, moments of inertia, and tire parameters) are known to be within specified value ranges.
    
  Spec:ODDm) The loads inside the EgoV are within the limits specified by the respective vehicle type.
    
  Spec:ODDn) There are no external loads attached to the EgoV, such as a trailers, roof racks, or any type of load carriers. 




§ INITIAL CONCEPT OF THE MPC-TP


The specifications for the MPC-TP are based on the overall architecture in Figure <ref>. The MPC-TP is (initially) given by the single block displayed in Figure <ref>. The specifications concern the input signals, the output signals, and the contents of the block itself, and they are presented along this structure in the remainder of this section.





 §.§ Input Interface / Perception


For the interface to the map fusion module, the 6 layers road model specified in ISO 34503 <cit.> is considered. See Figure <ref> for an illustration.






  ) Map Fusion: The MPC-TP receives a layered map every 50 ms, including

    
  Spec:MapFusiona) geometry and topology of the road, lane and road boundaries (layer 1),
    
  Spec:MapFusionb) traffic signs, speed limits, traffic guidance, construction barriers (layer 2),
    
  Spec:MapFusionc) temporal modifications, e.g., due to construction sites (layer 3).


For the sake of the SOTIF-EooC, it is assumed that all of this map data is always fully accurate. The MPC-TP must plan its trajectories such that they conform to the applicable traffic rules (e.g., speed limit, no passing zone).



  ) Object Fusion and Prediction: The MPC-TP receives a complete list of dynamic objects in its relevant vicinity every 50 ms, including

    
  Spec:ObjectFusiona) other traffic participants (layer 4),
    
  Spec:ObjectFusionb) animals and other temporary obstacles, like dropped cargo, rocks, etc. (layer 4).


All dynamic objects include additional information about their classification (vehicle, truck, motorcycle, pedestrian, animal type, obstacle, etc.), position, velocity, and heading direction, if applicable. Furthermore, a reliable prediction for all of these objects is provided over the next 6 seconds. The uncertainty in these predictions is accounted for by the use of intervals (e.g., for the velocity) or bounding boxes (e.g., for the position).

No assumptions are made on specific sensors, their field-of-view, or their operational status. All assumptions are based on the interfaces to the sensor fusion modules. If the information flow on these interfaces cannot be guaranteed, the vehicle-level function will hand over to the driver (SAE levels 1,2,3) or transfer the vehicle to a safe state (SAE levels 4,5).



  ) Ego Motion Fusion: The MPC-TP receives relevant information about the states and parameters of the ego vehicle every 50 ms, along with a with maximum estimation error (marked with a “Δ”):

    
  Spec:EgoFusiona) the relative position of the ego vehicle on the map x,y (Δ x, Δ y),
    
  Spec:EgoFusionb) the ego vehicle velocity v_lon,v_lat (Δ v_lon,Δ v_lat),
    
  Spec:EgoFusionc) the ego vehicle's global heading angle ψ (Δψ),
    
  Spec:EgoFusiond) the ego vehicle's acceleration a_lon,a_lat (Δ a_lon,Δ a_lat),
    
  Spec:EgoFusione) the vehicle's mass m (Δ m),
    
  Spec:EgoFusionf) the vehicle's moment of inertia around the vertical axis I_z (Δ I_z),
    
  Spec:EgoFusiong) the position of the center of gravity relative to the midpoint of the rear axle x_cog, y_cog, z_cog (Δ x_cog, Δ y_cog, Δ z_cog),
    
  Spec:EgoFusionh) the cornering stiffness of the front and the the rear tires C_α,f, C_α,r (Δ C_α,f, Δ C_α,r).



  ) Tracking Controller: Further information about the ego vehicle, in particular about the states of the lower-level actuators, are provided to the MPC-TP through the Tracking Controller every 50 ms:

    
  Spec:TrackControla) the front wheels' steering angles δ_f,l, δ_f,r,
    
  Spec:TrackControlb) the current engine torque T_eng,
    
  Spec:TrackControlc) the total gear ratio i_gear,
    
  Spec:TrackControld) the effective brake torque T_brake.
    

Actuator failures, controller failures or performance degradations are not considered in this paper, as the focus is on SOTIF instead of functional safety.


  ) Environment Fusion: The MPC-TP receives accurate information about the environmental conditions every 50 ms, including

    
  Spec:EnvironmentFusiona) conditions of the road surface, in particular the friction coefficient μ (Δμ) (layer 1),
    
  Spec:EnvironmentFusionb) weather conditions and lighting conditions, in particular the side wind speed v_wind (Δ v_wind) (layer 4).


In reality, environment parameters, such as the friction coefficient or the side wind speed, are variable over space and/or time. For simplicity, however, the architectures presumes the communication of constant values, which characterize the entire drivable area of the ego vehicle over the relevant prediction time, i.e., in this case 6 seconds. The tolerance thus includes the measurement uncertainty as well as the true temporal and/or spacial variations, if applicable.



  ) Tactical Planner: The MPC-TP receives tactical choices from the Tactical Planner every 50 ms, containing

  
  Spec:TacticalPlanninga) the desired target lane (e.g., the current lane, the left or right lane), 
  
  Spec:TacticalPlanningb) a desired reference speed (e.g., because the vehicle is about to leave the highway on the next exit),
  
  Spec:TacticalPlanningc) optional information about the desired lateral position on the target lane (e.g., deviation from the lane center line),
  
  Spec:TacticalPlanningd) optional information about desired distances to other vehicles or objects (e.g., the desired gap to the leading or trailing vehicle).



All requests by the Tactical Planner are facultative. For example, if the Tactical Planner requests a lane change, it is the task of the MPC-TP to evaluate if this lane change is feasible and safe, given the dynamics of the vehicle and the traffic situation (dynamic and static obstacles, traffic rules, etc.). If it is not feasible or not safe, the MPC-TP resorts to a trajectory that keeps the EgoV on its current lane. Similarly, the desired distances in (Spec:TacticalPlanningc,d) are for performance purposes only, and not required for safety.

The tactical choices are generated by the active driving function. For example, for the E^2LCACC the unique command is always to keep the current lane, for the ALCA the lane change commands are issued by the driver, and for the HP the commands are issued by a dedicated software module.





 §.§ Output Interface / Reference Trajectory


The main output of the MPC-TP is a reference trajectory, as shown in the architecture in Figure <ref>. The reference trajectory consists of (i) a trajectory header and (ii) a trajectory body. The trajectory header contains a sequential number, the generation time, a validity time interval, and a checksum.



  ) Trajectory Body: The main part of the reference trajectory is the trajectory body. It is represented by N_p-1 linear segments, as illustrated in Figure <ref>. Each segment, numbered with i=1,…,N_p-1, carries the following data:

    
  Spec:TrajectoryBodya) x_i,y_i: the planar coordinates of the starting point of the segment,
    
  Spec:TrajectoryBodyb) ψ_i: the heading angle of the segment,
    
  Spec:TrajectoryBodyc) v_i: the reference speed of the vehicle along the segment,
    
  Spec:TrajectoryBodyd) δ_i: the reference steering angle along the segment,
    
  Spec:TrajectoryBodye) l_i: the length of the segment.
    
  Spec:TrajectoryBodyf) In addition, the final point of the trajectory is given in terms of its planar coordinates x_N_p,y_N_p.


Note that the trajectory body data are partially redundant. This is mainly to avoid unnecessary and mirrored re-calculations within the MPC-TP.





 §.§ Specification and Design of the MPC-TP


This case study considers a trajectory planner based on Model Predictive Control (MPC). A generic MPC formulation with a mainstream application to highway driving is used, in order to make the SOTIF concept in this paper as generally applicable as possible. It should be remarked, however, that numerous variations of MPC-based planning algorithms have been proposed in the recent literature <cit.>. Moreover, commercial modules for trajectory planning with MPC have been made available, such as the software ProDriver by Embotech <cit.>. It assumes a similar functional architecture as the MPC-TP in this paper, with a sampling time of 100 ms. 

The computations of the MPC-TP are based on an internal prediction model. Following <cit.>, a  dynamic bicycle model with a linear tire model (LDBM) is selected and discretizes with an appropriate numerical integration method (e.g., Runge-Kutta):

    𝐱_t+1=f(𝐱_t,𝐮_t) .

The model is augmented with a suitable patch to circumvent the singularity of the tire forces at the origin. Here t=0,1,2,… denote the discrete time steps, corresponding to multiples of the fundamental sampling time t_s=50 ms. The vector 𝐱_t∈ℝ^6 includes the (model) states at step t, namely

    
  * s_t,e_t: coordinates of the vehicle's reference point in road-aligned (Frenét) coordinates, 
    
  * ψ_t,ω_t: yaw angle and yaw rate of the vehicle,
    
  * v_t,u_t: longitudinal and lateral velocity of the vehicle.

The vector 𝐮_t∈ℝ^2 contains the (control) inputs at step t, namely

    
  * δ_t: the steering angle of the front wheel,
    
  * M_t: the torque of the drivetrain at the wheels, used to accelerate or decelerate the vehicle.

Furthermore, a variable for the reference lane n_t is used to indicate the lane decision for the vehicle at time t. It is assumed that the lanes are numbered from right to left, starting with 1 and counting up to the total number of lanes. The set of available lanes at step t is denoted ℕ_t.

Based on the prediction, the concept of the MPC-TP is to set up and solve the following Optimal Control Problem (OCP) in each time step, using an appropriate numerical optimization solver:

    n_t,𝐮_t,𝐱_tminimize      ℓ_s(s_N_p)+∑_t=1^N_pℓ_e(e_t,n_t)+ℓ_o(s_t,e_t)+ℓ_u(δ_t,M_t) ,
    s.t.      𝐱_t+1=f(𝐱_t,𝐮_t)    ∀ t=0,1,…,N_p-1 ,
       𝐱_0=𝐱̅_0 ,
       𝐧_t∈ℕ_t    ∀ t=1,2,…,N_p ,
       𝐮_t∈𝕌    ∀ t=0,1,…,N_p-1 ,
       𝐱_t∈𝕏^(s)_t ,  𝐱_t∈𝕏^(t)_t    ∀ t=1,2,…,N_p-1 ,
       𝐱_N_p∈𝕏_f .

The objective function (<ref>a) comprises the following cost terms:

    
  * ℓ_s(s_N_p): rewards the progress along the road made by the vehicle over the prediction horizon;
    
  * ℓ_e(e_t,n_t): penalizes the lateral deviation of the vehicle from the center line of the corresponding reference lane;
    
  * ℓ_o(s_t,e_t): repels the vehicle from static and dynamic obstacles, e.g., by means of a potential field;
    
  * ℓ_u(δ_t,M_t): favors a low input usage and smooth changes in the inputs, e.g., for driver comfort and energy efficient driving.

These cost terms are multiplied by individual tuning weights, which are selected during the control design process.

The initial state 𝐱̅_0 of the LDBM (<ref>b,c) is obtained from the module for Ego Motion Fusion. The lane variables n_t over the prediction horizon in (<ref>d) are either a decision variable of the OCP (as for the HP), or they can be pre-determined before solving the OCP (as for E2LCACC and ALCA). Equations (<ref>e,f,g) represent the input constraints, state constraints, and terminal constraints, respectively. The state constraints are split into two sets:

    
  * The safety constraints 𝕏^(s)_t avoid collisions of the EgoV with static and dynamic objects, most importantly other vehicles, called target vehicles (TgtVs). Furthermore, the constraints ensure the validity of the tire model, by restricting the tire forces to remain within the admissible bounds of the linear tire regime (i.e., no slip in the tire-road contact patch).
    
  * The traffic rule and comfort constraints 𝕏^(t)_t represent the constraints from traffic rules (e.g., the EgoV must stay within its lane, the EgoV must respect the speed limit) and passenger comfort (e.g., no excessive acceleration or jerk).

The safety constraints are essential and cannot be relaxed, whereas the traffic rule and comfort constraints can be relaxed in emergency situations. Furthermore, in emergency situations, the LDBM in (<ref>) can be replaced with a nonlinear dynamic bicycle model (NDBM), which uses a Pacejka tire model instead of the linear tire model. By these modifications, the MPC-TP may temporarily operate the vehicle close to its dynamic limits and against the traffic rules, e.g., driving in between two lanes, or without considering passenger comfort.



  ) Further specifications related to the MPC formulation include the following:

    
  Spec:MPCa) The fundamental sampling time is t_s=50 ms.
    
  Spec:MPCb) The planning horizon is N_p=120 steps, which corresponds to a prediction time of T_p=6 s.
    
  Spec:MPCc) The input constraint set 𝕌 represents all limits of the steering, engine and braking systems, as well as the relevant limits of their rate-of-change.
    
  Spec:MPCd) The state constraint sets 𝕏_t are time-dependent and prevent collisions with static and dynamic obstacles, with the exception of vehicles driving behind the ego vehicle on the same lane. Moreover, the state constraints keep the vehicle away from its dynamic limits and ensure satisfaction of the traffic rules, e.g., by preventing forbidden lane changes or violation of the speed limit.
    
  Spec:MPCe) The terminal constraint 𝕏_f corresponds to a safe state of the vehicle in which it can remain indefinitely (e.g., at standstill), or it represents a safe exit strategy (e.g., maximum deceleration until standstill). Its purpose is to ensure the recursive feasibility of the OCP.



The numerical computations of the MPC-TP comprise two main steps:

    
  * setup of the OCP problem,
    
  * solution of the OCP problem.

Step 1 is to parse the OCP into the standard form of a nonlinear programming problem, which is realized by handwritten code. Step 2 is to solve the nonlinear program using a generic optimization solver, for which a generous selection of software packages is available <cit.>. 


    
  ) Besides the optimal trajectory, the optimization solver returns an exit flag q_exit in exactly one of four classes: 

    
    
  q_exit=1: A (local) minimum is found with an acceptable tolerance level.
    
  q_exit=2: A feasible point is found, but without an optimality property (e.g., because the gradient / search direction becomes too small or the solver has reached the maximum number of iterations / solution time).
    
  q_exit=3: The solver detects that no feasible point exists (`certificate of infeasibility').
    
  q_exit=4: A feasible point is not found, but without a certificate of infeasibility (e.g., because the solver has reached the maximum number of iterations / solution time), or an identified or unidentified solver error occurred (e.g., due to insufficient memory).



In reality, most solvers provide further diagnostics of the results, i.e., their exit flag is more detailed than the four classes above. The requirement (Spec:ExitFlag) states that it must be possible to map the true exit flag of the solver uniquely to one of the indicated classes. Moreover, it is assumed that the exit flag correctly characterizes the result of the solver. This can be verified by a separate solution checker. Cases where the exit flag is incorrect are not considered further as part of the SOTIF analysis. They represent software fault and hence belong to the realm of FS.



 §.§ Hazards at the Vehicle Level, Safety Goals, and SOTIF Requirements


For the SOTIF analysis in this paper, the following hazards at the vehicle level are considered:


    
    
  ) The ego vehicle leaves the assigned lane.
    
  ) The ego vehicle collides with other vehicles or obstacles.

These hazards lead directly to the specification of the following safety goals:


    
    
  ) The ego vehicle shall not leave the assigned lane(s).
    
  ) The ego vehicle shall not collide with other vehicles or obstacles.


As part of a top-down safety lifecycle, these safety goals would be broken down recursively into more and more detailed safety requirements until one arrives at safety requirements for the component in question. Safety requirements for an ADS comprise both functional safety requirements and SOTIF-related safety requirements (or SOTIF  requirements in short).
Such a refinement of safety requirements does not take place when developing a SOTIF-EooC. Instead, safety requirements must be assumed for the out-of-context element under consideration. The SOTIF-SEooC's fulfillment of these assumed safety requirements represents its contribution to achieving the original safety goals.

For the MPC-TP, we assume the following SOTIF-related safety requirements: 


    
  ) The MPC-TP shall provide a reference trajectory within t_c=30 ms.[The computation time is less than the sampling time t_s=50 ms, in order to account for additional communication delays.]
    
  ) The reference trajectory provided by the MPC-TP has to be consistent regarding the redundant information in the trajectory body. 
    
  ) The reference trajectory provided by the MPC-TP has to be admissible, in the sense that it must satisfy all relevant actuator constraints:

    
  SR:Admissiblea) The minimum / maximum longitudinal acceleration of the vehicle is a_lon,min=-10 m/s^2, a_lon,max=+3 m/s^2.
    
  SR:Admissibleb) The minimum / maximum velocity of the vehicle is v_min=0 km/h, v_max=130 km/h.[The minimum velocity assumption means that the EgoV may not drive in reverse. A maximum velocity is required by the limited forward view of the vehicle's sensor suite.]
    
  SR:Admissiblec) The maximum front wheel steering angle of the vehicle (in each direction) is δ_max=34^∘.
    
  SR:Admissibled) The maximum front wheel steering rate (in each direction) is γ_max=68^∘ 1/s.
    
  SR:Admissiblee) The maximum lateral acceleration of the vehicle (in each direction) is a_lat,max=5 m/s^2.



Similar to the upstream perception modules, no detailed technical specifications have made for the downstream Tracking Controller (TC). As it is developed independently, responsibilities have to be clearly allocated and distributed between the MPC-TP and the TC. Thus requirements have to be specified for the interface, i.e., in this case, the reference trajectory. The approach proposed in this case study is to require “drivability” based on a nonlinear dynamic double-track model (NDDM):

    
  ) The reference trajectory provided by the MPC-TP shall be drivable, i.e., it must be possible to track it with a NDDM using some arbitrary controller, such that all of the following conditions are satisfied:

    
  SR:Drivablea) The initial condition of the NDDM corresponds to the measured initial condition of the vehicle.
    
  SR:Drivableb) The parameters of the NDDM correspond to those of the vehicle (mass m, moment of inertia I_z, position of the center of gravity x_cog and y_cog, front and rear cornering stiffness C_α,f and C_α,r).
    
  SR:Drivablec) The tires of the NDDM do not saturate, i.e., there is no slip in the tire-road contact patch, for the worst-case friction coeffient μ-Δμ. This means that the tire forces remain inside the linear regime at all times.
    
  SR:Drivabled) The bank angle is between β-Δβ and β+Δβ.
    
  SR:Drivablee) The side wind speed is between v_wind-Δ v_wind and v_wind+Δ v_wind.
    
  SR:Drivablef) All existing actuator constraints are respected: communication delays, actuator dynamics, actuator limits, and limits of the rates of change.
    
  SR:Drivableg) The maximum lateral offset from the reference trajectory is e_max=5 cm.
    
  SR:Drivableh) The length of each reference trajectory is such that it takes at least 5 s to reach the end.



The set of all consistent, admissible and drivable trajectories is denoted as 𝒯. Additionally, the trajectory should be such that it does not lead to collisions of the vehicle.


  ) The trajectory shall be collision-free, i.e., it must stay within the boundaries of the road and it must avoid collisions with all dynamic obstacles. 

    
  SR:CollisionFreea) For lateral collsition avoidance, the MPC-TP may rely on a maximum lateral offset e_max^TC produced by the TC:
    
    
  * for v≤ 30 km/h: e_max^TC=10 cm,
    
  * for v≤ 80 km/h: e_max^TC=15 cm,
    
  * for v≤ 130 km/h: e_max^TC=20 cm.
    
    
  SR:CollisionFreeb) The MPC-TP shall plan its trajectories such that it is always safe for the ego vehicle to stay in its current lane, in particular by maintaining an appropriate safety distance to the leading vehicle.

 
To this end, the MPC-TP may assume that all of the input information it receives is up-to-date and fully correct. 

Note that the scope of the paper is limited with regards to the considered vehicle level hazards and SOTIF requirements. In reality, there may be further hazards and derived requirements, which may involve also other aspects of the lateral motion, a such as acceleration and jerk, as well as the longitudinal motion of the vehicle, e.g., for overtaking maneuvers.



§ SOTIF BACKGROUND




 §.§ Aspects of Safety


The safety of a technical system is commonly defined as the absence of unreasonable risk for the health and life of humans <cit.>. A reasonable level of risk has to account for the fact that most technological systems, including ADSs, can never be entirely safe. Safety risks of E/E systems can be categorized according to the source of potential hazards into

    
  1) hazards that occur due to malfunctioning behavior (Functional Safety), addressed by the ISO 26262 <cit.> standard, and
    
  2) hazards that occur due to functional insufficiencies (Safety of the Intended Functionality, SOTIF), addressed by the ISO 21448 <cit.> standard.

Functional safety and SOTIF are distinct and complementary aspects of safety that both need to be accounted for in the development of an ADS <cit.>. The exclusive focus of this paper, however, is SOTIF.
 



 §.§ SOTIF


The international standard ISO 21448 'Road vehicles - Safety of the intended functionality' published in 2022 defines SOTIF as the absence of unreasonable risk due to hazards resulting from functional insufficiencies of the intended functionality, or its implementation <cit.>. As a basic framework, it defines an iterative lifecycle with several activities that are deemed adequate for developing an ADS that satisfies the SOTIF, as shown in Figure <ref>.
















 §.§ SOTIF-related Element out of Context (SOTIF-EooC)
 

As a consequence of the collaborative development typical for the automotive industry, a supplier company often provides a component that will be integrated by the OEM into a SOTIF-related system. Under this scenario, at the time of developing the component, the the supplier may only know certain aspects of the overarching system. One way of facilitating the achievement of SOTIF in such a context is to treat the component as a SOTIF-related Element out of Context (SOTIF-EooC), according to ISO 21448 <cit.>.   

For a component treated as a SOTIF-EooC, assumptions can be made regarding its use within the whole system and its contribution to the intended functionality. Based on this, assumptions about the SOTIF-related functional insufficiencies (FIs) and their allowed target rate of occurrence can be derived. These assumptions need to be documented. They are used as inputs for the subsequent development of the SOTIF-EooC by the supplier. For the SOTIF-EooC, the identified triggering conditions (TCs) of the component and their resulting FIs are documented together with the assumptions of use for the component (a.k.a. integration requirements).
When the OEM integrates the SOTIF-EooC, the validity of the assumptions or integration requirments is established by  activities in the context of whole vehicle-level functionalities, according to ISO 21448, Clause 4.4.3 <cit.>.

In this paper, the MPC-TP is treated as a SOTIF-EooC. 
In fact, the MPC-TP may be used as part of different driving automation systems, namely E^2LCACC, ALCA, and HP. Assumptions regarding the overall systems, the integration of the MPC-TP and the contributions of the MPC-TP to the intended functionality have been discussed in Sections <ref> and <ref>.
The identification of TCs of the MPC and their resulting FIs are discussed in Section <ref>. Naturally, their analysis leads to an improved architecture and refined specifications, which are presented in Section <ref>.





 §.§ Functional Insufficiencies


r0.55
	

	
    < g r a p h i c s >

	

SOTIF scenario categories.
	



FIs lead to hazards only in specific (driving) scenarios, called the hazardous scenarios, as opposed to non-hazardous scenarios. Conceptually, these scenarios are further sub-divided into known and unknown scenarios. This categorization of scenarios is illustrated in Figure <ref>. Hazardous scenarios may be known or unknown at the start of the  analysis. The overall goal of SOTIF is to improve the specification and the design of the system, or the considered element, with respect to reducing the number of (known and unknown) hazardous scenarios. 









FIs may exist at the system level, e.g., the E2LCACC system, or at the element level, e.g., the 'Sense', 'Plan', or 'Act' parts of the system. Since FIs can lead to hazardous behaviour, their identification and the evaluation of their effects are key activities in the development of a SOTIF related item <cit.>. ISO 21448 subdivides FIs into

    
  * specification insufficiencies, which refer to a (possibly incomplete) specification contributing to hazardous behavior, and
    
  * performance insufficiencies, which refer to limitations of the technical capabilities contributing to hazardous behavior <cit.>.
 



Examples of specification insufficiencies include an incomplete specification of the E2LCACC's headway distance resulting in the ego vehicle not keeping a safe distance to the vehicle in front or the inability of the HP to handle uncommon freeway road signs that were not part of its specification and thus cannot be processed appropriately by the system.
Examples of performance insufficiencies include limitations of technical capabilities of the ALCA such as limited calculation performance, limited perception range of its front radar sensor, or limited actuation capabilities of the foundational steering system  <cit.>.

As this paper is concerned with an MPC-TP for different driving functions, the focus is on FIs at element level, in this case, of a “Plan” component. 





 §.§ Triggering Conditions


Hazardous scenarios can be decomposed into the following elements: (1) A traffic scenario with a triggering condition, (2) which leads to a hazardous behavior of the ego vehicle, (3) which may cause a hazard in combination with the surrounding traffic or environment. Hence a TC is defined as a specific condition of a scenario that initiates a subsequent system reaction contributing to hazardous behavior <cit.>(simplified). 


TCs and FIs are interrelated: A TC of a scenario activates a FI, which results in a subsequent system reaction. Figure <ref> illustrates the activation of FI at the element level by a TC and the resulting hazardous behavior. In general, the SOTIF standard also addresses reasonably foreseeable misuse. This aspect however, remains outside the scope of this paper.



For an example, assume an activated E2LCACC system that uses a radar sensor with an insufficient field of view in cased of curved roads. The upper part of Figure <ref> shows a scenario without a TC. A FI is not being activated. The E2LCACC system works as intended, i.e., the scenario is considered not hazardous.  The lower part of Figure <ref> shows a second, hazardous scenario. Here, the TC is a slower moving vehicle ahead in lane on a curved road. In this case, the assumed FI at the sensor is activated, resulting in a late detection of the leading vehicle. As a result, the system brakes too late, resulting in a rear-end collision with the leading vehicle.   








§ SOTIF ANALYSIS FOR THE MPC-TP




 §.§ Identification and Evaluation of Functional Insufficiencies and Triggering Conditions



ISO 21448 suggests a list of specific methods to analyze potential FIs and TCs <cit.>. Table <ref> shows the list of methods that are selected as appropriate to derive FIs and/or TCs of the MPC-TP, and the ones that are considered further in the remainder of this paper.



For the analysis of the “Plan” component of a system, ISO 21448 <cit.> provides an additional, specific list of methods that can be used for finding FIs and TCs. This list is provided in Table <ref>. The categories considered for the SOTIF analysis in this paper are shown in black, the ones not considered further are shown in gray.



This section describes the application of the selected analysis methods, and the FIs and potential TCs that have been identified. Multiple methods may lead to the same results, in which case the corresponding FIs and TCs are identified with the same number.



 §.§ Analysis of ODD, Use Cases, and Scenarios


This analysis method involves a close examination of the ODD specifications in (Spec:ODD), as well as predictable use cases and scenarios that may lead to potential TCs and FIs of the MPC-TP. Using this approach, the following items have been identified for our case study:  


In congestion situations (TC:AOUCS01), the MPC-TP does not consider the formation of a rescue alley for police or ambulances (FI:AOUCS01). Conversely, the formation of a rescue alley by other vehicles or the presence  of police or ambulances in congestion situations may lead the MPC-TP to perform unusual, illegal, counter-intuitive or even dangerous maneuvers (FI:DangerousManeuver).


In obstacle avoidance scenarios (TC:AOUCS02), the MPC-TP does not allow emergency maneuvers without a proper lane change. For example, it is not possible to avoid an obstacle by driving in between two lanes or by using the shoulder (FI:AOUCS02). Conversely, the emergency obstacle avoidance by other vehicles may lead the MPC-TP to perform counter-intuitive or even dangerous maneuvers (FI:DangerousManeuver).


There is no speed limit on certain sections of the Autobahn. Suppose there is blocking obstacle on the road, such as the rear end of a congestion. If the obstacle is at standstill shortly beyond the prediction horizon of the MPC-TP (TC:AOUCS03) and an obstacle avoidance maneuver is not possible, e.g., by performing a lane change. If the EgoV exceeds a certain speed v_max, this can lead to a rear-end collision because the prediction horizon N_p is not long enough for the EgoV to come to a full stop within it (FI:AOUCS03). 






The design of the current MPC-TP is based on clearly defined traffic rules. It assumes, that surrounding vehicles either have the right-of-way, and are thus considered as obstacles, or they have to yield, and can thus be ignored. However, there are situations where the right-of-way is unclear, such as at the end of two merging lanes (TC:AOUCS04a). Without further measures, this may lead to an overly conservative driving behavior (FI:AOUCS04a), and even a complete deadlock for the EgoV (FI:AOUCS04b).


On freeways, cars typically only drive forwards. However, in exceptional situations, e.g., during congestion or due to police or ambulance operations, it may be necessary to perform reverse maneuvers. Moreover, surrounding vehicles may drive backwards (TC:AOUCS05), in which case the MPC-TP may become infeasible (FI:Infeasible) if the EgoV is at rest or slowly moving forwards.


In the case of a wrong way driver (TC:AOUCS06), the MPC-TP may be unable to compute a feasible trajectory (FI:Infeasible). Or it may lead the MPC-TP to perform counter-intuitive or even dangerous maneuvers (FI:DangerousManeuver).


The MPC-TP shall be able handle situations with an unexpected, sudden change of road conditions. Important examples include a dropping friction coefficient, e.g., due to water / hydroplaning (TC:AOUCS07a), snow / ice (TC:AOUCS07b), or oil (TC:AOUCS07c), or an irregular surface, e.g., due to gravel or stones (TC:AOUCS08a), pot holes (TC:AOUCS08b), or road bumps (TC:AOUCS08c). In such situations, the trajectory generated by the MPC-TP may actually be not drivable, in the sense of (SR:Drivable), e.g., because of the limitations of the LDBM using a linear tire model (FI:Drivable).





 §.§ Analysis of System Architecture (Including Redundancies)


This analysis is based on the system architecture, as depicted in Figure <ref>. The goal is to identify potential FIs and TCs based on the overall behavior of the entire system.

The receding-horizon implementation causes the MPC-TP to act also as a feedback controller for the vehicle, which is a well-known feature of MPC <cit.>. The interoperation of the MPC-TP with the tracking controller (TC) may thus lead to undesired bahavior, such as oscillations (FI:ASA01), or even instability of the EgoV motion. This may be triggered, for example, by external disturbances such as a side wind gust, e.g., on a bridge deck (TC:ASA01), or irregular, non-continuous lane markings, e.g., due to suddenly disappearing (TC:ASA02a), dirty (TC:ASA02b), broken (TC:ASA02c), or covered (TC:ASA02d) lane markings.

Difficulties may also appear in situations where the right-of-way is unclear (TC:AOUCS04), or when other traffic participants do not strictly adhere to the traffic rules (TC:ASA05). Their interaction with the MPC-TP may then lead to an overly conservative behavior of the EgoV (FI:AOUCS04), possibly resulting in a poor progress on the road (FI:AOUCS04a), or even a deadlock (FI:AOUCS04b).

The cut-in of a faster TgtV in front of the EgoV (TC:ASA04) may lead to a violation of the front safety distance, and hence the inability of the MPC-TP to generate a feasible solution (FI:Infeasible). This can be resolved by relaxing the minimum safety distance constraint in case of a leading vehicle with a higher velocity. Even if the safety distance is not violated, the cut-in of a faster TgtV (TC:ASA04) may still lead to an overly conservative braking maneuver on behalf of the EgoV (FI:AOUCS04c).


The MPC-TP does not consider a trailing vehicle driving closely behind the EgoV in the same lane (TC:ASA06). According to (Spec:MPCd), even though it is the responsibility of the trailing vehicle to keep a sufficient safety distance, sudden braking maneuvers may lead to a collision with the trailing vehicle and are thus unsafe (FI:ASA03).




 §.§ Analysis of Functional Dependencies


In the context of this case study, functional dependencies are interpreted as other modules that are actually or potentially connected with the MPC-TP, be it upstream or downstream. The MPC-TP needs to cope with challenges arising from the limitations of these modules. With this interpretation of functional dependencies, TCs and FIs identified with this analysis method may overlap with those identified by analyzing the system architecture (in Section <ref>), in the context of the entire system.


Dynamic obstacles suddenly appearing in the field of view of the system (TC:AFD01), may overwhelm the ability of the MPC-TP to solve the OCP in order to compute a new trajectory in due time (FI:InsufficientTime), violating (SR:Timing), or to find a feasible trajectory at all (FI:Infeasible), violating (SR:CollisionFree). Examples of this group of FIs are a previously shadowed traffic participant (TC:AFD01a), an object dropping from a bridge or a truck (TC:AFD01b), or a large animal (e.g., a wild boar) crossing the freeway (TC:AFD01c).  


At points of sudden traffic rule changes (TC:AFD02), such as the beginning of a speed limit area or a no passing zone, the MPC-TP may also unable to compute a feasible trajectory (FI:Infeasible), or may be unable to solve the OCP within the allotted time (FI:InsufficientTime). 





 §.§ Analysis of Algorithms and Their Outputs or Decisions


In the context of the MPC-TP, this analysis pertains to the formulation of the OCP (<ref>) and the numerical optimization algorithm, the decisions taken by the MPC-TP, as well as the reference trajectory and the exit flag as its outputs.

The OCP is based on the LDBM, whose linear tire model may represent a poor approximation of a real vehicle if essential model assumptions are violated (FI:AAOD01). The validity of the linear tire model is already supported by corresponding constraints 𝕏^(s)_t (see Section  <ref>). The other main assumption of the LDBM is a non-negligible roll motion of the vehicle. It can be violated by an extremely asymmetric load (TC:AAOD01a), a highly dynamic cornering maneuver (TC:AAOD01b), or a high position of the CoG (TC:AAOD01c). 


Potential problems arising from the interoperation of the MPC-TP with the TC and from the lack of interaction-awareness have already been discussed (see Section <ref>). In addition, the MPC-TP may cause a lack of string stability in a chain of vehicles (TC:StringStability). If the leading brakes  abruptly, this can lead to the build-up of an increasing wave of braking maneuvers by all following vehicles. This string instability may be fostered by the MPC-TP in the middle of a chain of human-driven or automated vehicle, which may lead to collisions (FI:StringStability), if the MPC-TP is not properly tuned. 


The outputs of the MPC-TP include the reference trajectory and the solver exit flag (see Figure <ref>). 


Without software or hardware faults, the computed reference trajectory will be consistent, admissible, drivable, and collision-free, if the set 𝒯 of such trajectories is non-empty. However, due to the finite prediction time of T_p=6 s, the MPC-TP may lead the vehicle to an (unsafe) situation that cannot be resolved by a future trajectory, i.e., where 𝒯 becomes empty. In MPC language, this is known as the lack of recursive feasibility (FI:RecursiveFeasibility). A possible TC is the fast approach of a slowly driving TgtV in the front of the EgoV, where the TgtV cannot be avoided by a lateral maneuver (TC:SlowLeadingTgtV).

The solver exit flag itself might provide an indication of a triggered FI. In particular, all exit flag values q_exit≠ 1 should be considered carefully:

    
  * q_exit=2 indicates that the MPC may exceed the allotted computation time (FI:InsufficientTime), but produces a feasible trajectory. This could be triggered by rapidly changing traffic situations (TC:AAOD02) or by suddenly appearing obstacles (TC:AFD01). 
    
  * q_exit=3 means that the MPC-TP is unable to generate a feasible (collision-free) trajectory for the given constraints, because 𝒯 is empty (FI:Infeasible). For example, this can be triggered by (TC:AOUCS05), (TC:AOUCS06), (TC:AOUCS07), (TC:AOUCS08), (TC:ASA04), (TC:AFD01),(TC:AFD02).
    
  * q_exit=4 means that the solver is unable to find a solution (FI:NoSolution), even though 𝒯 may be non-empty. As software and hardware faults are out of scope, this must be due to unpredictable numerical problems (e.g., ill-conditioning, floating point arithmetics). To the best of the authors' knowledge, there are no known specific triggering conditions for this case.






 §.§ Analysis of Boundary Values


For the analysis of boundary values, the MPC-TP is examined in the context of scenarios where relevant parameters take on extreme values. Parameters considered in the analysis include

    
  * ODD and road parameters, i.e., the road bank angle, the friction coefficient μ, the road curvature, the change of curvature, and the wind speed;
    
  * maneuver parameters, i.e., the reference velocity, maximum lateral motion (e.g., due to a sudden lane change or obstacle avoidance), maximum longitudinal motion (e.g., due to emergency braking or maximum acceleration);
    
  * vehicle parameters, i.e., the vehicle mass (m), the moment of inertia (I_z), the position of the CoG (x_cog, y_cog, z_cog), the cornering stiffness of the front and the the rear tires (C_α,f, C_α,r), and all chassis and suspension parameters of the specified vehicle class.


The analysis is based on extensive simulations using a high-fidelity vehicle model, e.g, in CarMaker. The simulations scenarios aim to cover possible combinations of extreme values of the above parameters. The results are checked against the requirements (SR:Timing) - (SR:CollisionFree).

To this end, a database of artificial test scenarios covering corner cases, is generated, based on different incarnations of the maneuver parameters. Specifically for the reference velocity, scenarios are separated into low-speed (v≤ v_low=5 km/h), medium-speed (v≤ v_med=50 km/h), and high-speed (v≤ v_high=130 km/h) categories. For road curvature and change of curvature, only scenarios that (at least partially) involve maximum values are considered. The generation process of corner cases can also be automated, as proposed in Lubiniecki et al. <cit.>.

The database of corner cases is then simulated for extreme values of the ODD and road parameters and the vehicle parameters. Due to the combinatorial nature, the number of simulation scenarios thus becomes intractable. To regain tractability, only pairs of extreme parameter values are included in the boundary value analysis, while all other parameters are set to their default or regular values. For some parameters, such as the height of the CoG z_cog, instead of a default or regular values, it is more reasonable to consider only the maximum value in all simulations. Critical cases are expected to result from certain parameter combinations (in some or all corner cases), such as

    
  * the maximum side wind speed v_wind,max = 8 bft and the maximum bank angle β_max = 8^∘ (TC:CommonTrigger1),
    
  * the minimum friction value μ_min=0.15 and the minimum tire cornering stiffness C_α,f,min=60 kN/rad (TC:CommonTrigger2),
    
  * maximum front tire cornering stiffness C_α,f,max=200 kN/rad and the minimum rear tire cornering stiffness C_α,r,min=60 kN/rad, and vice versa (TC:CommonTrigger3).


These TCs may lead the MPC-TP to provide trajectories that are not drivable by a specific vehicle model or configuration (FI:Drivable). However, these are potential TCs and FIs, and their existence and relevance has to be confirmed by actually performing a large-scale, high-fidelity simulation study.



 §.§ Analysis of Common Triggering Conditions


Generally, common TCs are collected from prior experiences with the development of similar systems. They may be available, for example, by internal reports, industry standards, or other publicly documents. The TCs identified in this way may, of course, overlap with TCs identified using other methods.

The authors are currently not aware of a database or any other source for common TCs for a trajectory planner. However, applicable TCs for other ADSs from the relevant literature have been taken into account in this case study. In particular, selected TCs for a LKA system from <cit.> can analyzed with respect to their ability to challenge the MPC-TP and to trigger potential FIs:

    
  * lane keeping / lane changing scenario with suddenly disappearing lane markings (TC:ASA02a);
    
  * lane keeping / lane changing scenario with dirty, broken, or covered lane markings (TC:ASA02b,c,d);
    
  * lane keeping / lane changing scenario with unrecognizable lane markings due to direct sunlight or change of lighting conditions, e.g., when entering a tunnel (TC:ACTC03a,b);
    
  * insufficient visibility of lane markings due to low visibility, e.g., due to snow / ice or fog (TC:ACTC04a,b);
    
  * infrastructure items that could be misinterpreted as lane markings, e.g., curbs or dividers (TC:ACTC05a,b).

Other common TCs for a trajectory planner that can been identified from the existing literature include

    
  * side wind gusts, e.g., on a bridge deck (TC:ASA01a),
    
  * an uneven road surface, due to potholes (TC:AOUCS08b) or road bumps (TC:AOUCS08c),
    
  * a slippery road surface, e.g., due to a wet road with foilage (TC:ACTC09a) or a snowy / icy road (TC:ACTC09b).


For many of the common TCs from literature it remains unclear however, whether they activate FIs in the MCP-TP or elsewhere in the system. This needs to be verified in simulations or real-world experiments.



 §.§ Environment and Location


The systematic analysis of the environment and location aims to identify relevant freeway scenarios and warning signs that may compromise the safety of the MPC-TP. By means of brainstorming, the following scenarios in the ODD can be identified as potential TCs in this context.

An increase or decrease in the number of lanes on the freeway may present a particularly challenging situation for the MPC-TP, including:

    
  * entering the freeway from an on-ramp (TC:EAL01),
    
  * exiting the freeway via an off-ramp (TC:EAL02),
    
  * lane ending / lane merging situations (TC:AOUCS04a),
    
  * lane adding situations (TC:EAL04).


Similarly, situations with special vehicles or vehicles operating beyond traffic rules may pose particular difficulties for the MPC-TP, such as: 

    
  * a wrong way driver (TC:AOUCS06), 
    
  * a reckless driver or person driving under-the-influence (TC:EAL08),
    
  * an approaching emergency vehicle (TC:EAL05),
    
  * a heavy transport or oversized vehicles (TC:EAL06),
    
  * a vehicle parking on the freeway shoulder (TC:EAL07).



Furthermore, rare situations and uncommon road signs on the German Autobahn can be considered in the context of this analysis, leading to the following list of TCs:

    
  * uneven road surface (road sign 112), e.g., due to ruts or blow-ups (TC:AOUCS08d,e),
    
  * an invisible / indetectable substance is substantially lowering the friction coefficient, e.g., due to an oil spillage (road sign 114) (TC:AOUCS07c),
    
  * the freeway shoulder is not available (TC:EAL09) or the shoulder is to be used as an additional lane (road signs 223.1, 223.2, 223.3) (TC:EAL10),
    
  * end of the freeway (road sign 330.1) (TC:EAL11) or a freeway closure (TC:EAL12),
    
  * an unusually steep slope (road signs 108, 110) (TC:EAL13) or an usually high curvature (road signs 103-10, 103-20, 105-10, 105-20) of the road (TC:EAL14),
    
  * dangerous side wind gusts (road sign 117) (TC:ASA01),
    
    
  * potentially unknown traffic scenarios or traffic signs in other European countries (TC:EAL15).


All of the above TCs may affect the ability of the MPC-TP to solve the OCP and compute a trajectory in due time (FI:InsufficientTime), or to find a feasible (collision-free) trajectory at all (FI:Infeasible). Furthermore, they may cause the MPC-TP to provide trajectories which are not drivable and thus lead the EgoV to leave its assigned lane and/or collide with other vehicles or obstacles (FI:Drivable) or which may cause the vehicle to perform unusual, illegal, counter-intuitive or even dangerous maneuvers (FI:DangerousManeuver).



 §.§ Summary of Identified Triggering Conditions and Functional Insuffciencies


Appendix <ref> provides a table with an overview of all triggering conditions and functional insuffciencies that have been identified for the MPC-TP in this case study.



§ REFINED CONCEPT FOR THE VEHICLE LEVEL CONTROL SYSTEM AND THE MPC-TP


As per the SOTIF lifecycle, the SOTIF-EooC analysis in Section <ref> leads to a revision of the overall concept of the vehicle level control system and the MPC-TP, in particular. The results include a refinement of the MPC-TP specifications and design as well as the ODD, as explained in Sections <ref> and <ref>). As a further consequence, the integration requirements to the vehicle level control system are updated, as detailed in Section <ref>. Some situations that could not be (fully) resolved as part of this case study are described in Sections  and .



 §.§ Refined Specification and Design of the MPC-TP


The refined system architecture based on the SOTIF analysis in Section <ref> is depicted in Figure <ref>. The particular refinements of the specification and the design of the MPC-TP are discussed below.





  §.§.§ Design Improvements















The SOTIF-EooC analysis has led to the following concepts to improve the architecture and algorithmic design of the MPC-TP.

An overly conservative behavior of the EgoV (FI:AOUCS04c) may result, e.g., due to the cut-in of a faster TgtV (TC:ASA04). This potential issue can be mitigated by a proper tuning of the MPC-TP.

A trailing TgtV not keeping a proper safety distance to the EgoV (TC:ASA06) may cause a collision in case of a sudden braking maneuvers of the EgoV (FI:ASA03). 

a*) Even though this lies within the responsibility of the TgtV,  the EgoV shall perform sudden braking maneuvers only when strictly necessary and only to an appropriate extent. This can be accomplished by adding a strong penalty on the usage of high braking inputs in the OCP.

Spec:SuddenBrakingb*) Furthermore, the distance to the trailing vehicle shall aslo be included in the tuning of the MPC-TP.

The interaction with other traffic participants if the right-of-way is unclear (TC:AOUCS04) or if they do not strictly adhere to the traffic rules (TC:ASA05) may lead to an overly conservative behavior of the EgoV (FI:AOUCS04). 

*) This should be properly resolved by using an interaction-aware MPC approach, which is the subject of ongoing research <cit.>. For the given architecture, the interaction-aware MPC also has to be designed in coordination with the Tactical Planner.

Finally, a lack of string stability may be fostered by the MPC-TP in a chain of human-driven or automated vehicles (TC:StringStability), which may potentially lead to collisions (FI:StringStability). The problem, however, is not MPC-specific and has been analyzed in the previous literature, including ACC systems <cit.>. It shall be resolved by a proper tuning of the MPC-TP, which should be verified in extensive simulations and/or on-road tests.



  §.§.§ Adaptations to the Reference Trajectory


The interoperation of the MPC-TP with the lower-level TC may lead to oscillations and even unstable behavior (FI:ASA01). 
Spec:TrajectoryBodyg*) As a solution to this, the first N_r=4 steps of the previously computed trajectory remain unchanged, i.e., only the subsequent inputs are newly computed. N_r is called the reaction horizon. The concept is illustrated in Figure <ref>. The initial condition for the OCP in (<ref>c) thus becomes the predicted state x̂_N_r from the previous OCP solution.



As a negative consequence of this modification, the reaction time of the MPC-TP is increased. This is relevant, for instance, if the EgoV has to react to sudden events in its environment, such as in an emergency braking or obstacle avoidance scenario. However, a reaction time of T_d=N_rt_s=200 ms is still superior to that of an average human driver, which is typically between 600 ms and 1,000 ms, according to recent studies <cit.>. Thus a reaction time of 200 ms is deemed to be sufficient for common road traffic.



  §.§.§ Potential Extension of Computation Time


The regular MPC is designed for a maximum computation time of t_c=30 ms. This is less than the given sampling time, t_s=50 ms, in order to account for additional latencies, e.g., due to communication and actuation delays. Obviously, the choice of t_c should be based on generous computational margins and extensive testing. However, it is generally not possible to guarantee that the solver always provides a definite answer within this allotted computation time, i.e., that it finishes with an exit flag q_exit = 1,2,3 in due time (FI:InsufficientTime). Several TCs have been identified as part of the SOTIF analysis in Section <ref> that may represent a potential cause for an excess computation time, such as a suddenly appearing obstacle (TC:AFD01), sudden changes of traffic rules (TC:AFD02), or rapidly changing traffic situations (TC:AAOD02). A strategy to remedy this issue may look as follows:

Due to the concept of the reaction horizon, introduced in Section <ref>, the computation time can be extended by multiple (in this case: N_r=4) time steps. In occasional peaks, it can reach up to a theoretical maximum time of t̅_c=t_c+(N_r-1)t_s=180 ms. After that, a new trajectory must definitely be provided. If multiple time steps are used to solve the OCP, the new decisions (shown in red in Figure <ref>) comprise a corresponding number of time steps, which are now more than a single one.

Furthermore, the solver settings are adjusted such that the iterations can be halted after t_c, if they have not terminated; or again after each t_c+k t_s, for k=1,2,…,N_r-1. After each instance, the status of the current iterate can be checked. If the current exit flag equals to q_exit = 4, the solver may continue its computations from the current iterate, until finally the ultimate maximum solver time t̅_c is reached.





  §.§.§ Backup MPC


The Backup MPC (MPC-TP2) is a redundant implementation of the Primary MPC (MPC-TP1), which has been discussed thus far. The Backup MPC has the same inputs and outputs as the Primary MPC, as shown in Figure <ref>). It is designed according to the same specifications and requirements, as described in Sections <ref> and <ref>, except for the following differences:

    
  * The Backup MPC is utilized for safety and not passenger comfort. It has the ability to perform any maneuver that is physically possible. This means a different tuning of the weight parameters in the cost function and the traffic rule and comfort constraints 𝕏^(t)_t are removed from the OCP in (<ref>f).
    
  * Instead of a LDBM, which has a linear tire model, the Backup MPC shall include a NDBM with a Pacejka tire model. The NDBM provides a higher fidelity for the dynamics of the vehicle at the limits of handling, even though it may have a longer solution time.
    
  * For additional redundancy, the Backup MPC shall apply a different numerical solver (different algorithm and/or software implementation) and it shall be running on an independent computational platform.


The Backup MPC is constantly running in a shadow mode. During normal operation, when the Primary MPC is active, the solutions of the Backup MPC solutions are simply discarded. The trajectories of the Backup MPC are activated, or subsequently deactivated, according to a new module called Infeasibility Analysis. Its logic is based on the exit flag of the Primary MPC q_exit^(P), the exit flag of the Backup MPC q_exit^(B), and various information from the perception modules; see Section <ref> for further details. In particular, the Backup MPC shall be activated if the Primary MPC is unable to find a feasible solution (FI:Infeasible) under the following circumstances:

    
  * The friction coefficient suddenly drops by a value of μ_d>0 within a specified time interval t_d>0, due to water, snow / ice, or oil (TC:AOUCS07), or an irregular surface, e.g., due to gravel or stones, pot holes, or road bumps (TC:AOUCS08).
    
  * Static or dynamic obstacles suddenly appear close to the vehicle, e.g., due to a previously shadowed traffic participant (TC:AFD01a), a lost cargo or other object dropping from a bridge or a truck (TC:AFD01b), or a large animal (e.g., a wild boar) crossing the freeway (TC:AFD01c).
    
  * There is a sudden change of traffic rules (TC:AFD02), such as the beginning of a speed limit zone or a no passing zone.


The Backup MPC remains active until the Primary MPC produces a optimal solution again (case q_exit=1) and it is safe to switch back. For the latter condition, it is important to carefully avoid problems that may arise from a possible periodic switching behavior between the two planners.



  §.§.§ Infeasibility Analysis


The desirable outcome of the Primary MPC is a (local) minimum with an acceptable tolerance (q_exit = 1). The SOTIF analysis in Section <ref>, however, has revealed multiple scenarios where this may not be the case. Then a new module called the Infeasibility Analysis is activated. Its logic is designed as follows:

If the Primary MPC has not converged to such a minimum and returns only a feasible trajectory (q_exit = 2), the sub-optimal solution is acceptable on an exceptional basis. Because the sub-optimal solution satisfies also the terminal constraints, the recursive feasibility of the trajectory planning is not jeopardized. However, situations where the solver detects the definite infeasibility of the OCP (q_exit = 3) or where it is unable to find a feasible solution (q_exit = 4) require further attention. Here a switching logic, as presented in Table <ref>, is proposed as a basis for the Infeasibility Analysis.



Before even activating the switching logic, however, the Infeasibility Analysis filters a few special situations that will obviously or possibly lead to an infeasibility of the Primary MPC (FI:Infeasible). This includes 

    
  * (TC:AOUCS05) a leading TgtV driving in reverse, while the EgoV is at rest or slowly moving forward; 
    
  * (TC:ASA04) at higher speeds, the cut-in of a faster TgtV in front of the EgoV leading to a violation of the safety distance in the present time step.


For (TC:AOUCS05), a special planning mode shall be designed to check if (a) the EgoV is driving with a speed less than or equal to some threshold v_low (e.g., v_low=5 km/h) and (b) the minimum safety distance to the leading TgtV is already violated at the current time step. If this is the case, a stopping trajectory is automatically issued that transitions the EgoV to, or keeps it in, a complete standstill. When the Primary MPC becomes feasible again, the EgoV resumes its normal operation and moves forward. 

For (TC:ASA04), the situation can be resolved by an additional logic that relaxes the minimum safety distance constraint to a leading TgtV in the Primary MPC, if it has a higher velocity than the EgoV. Note that, even if the safety distance is not violated, the cut-in of a faster TgtV (TC:ASA04) may still lead to an overly conservative braking maneuver on behalf of the EgoV (FI:AOUCS04c). This is a separate issue that has to be resolved by a different measure.

In situations with a slippery road surface (TC:ACTC09) or a sudden drop of the friction value μ within a time interval of t_d for more than μ_d (TC:AOUCS07), in a first step the linear tire regime of the LDBM in (<ref>b) shall be exchanged with a (nonlinear) Pacejka tire model. This measure is taken pre-emptively in order to avoid a lack of drivability of the computed trajectory (FI:Drivablea). The Pacejka tire model is not used as the standard tire model of the Primary MPC because of its additional computational complexity.



According to Table <ref>, if the Primary MPC does not produce a feasible solution (q_exit^(P) = 3,4), the Backup MPC is activated already after 2 steps. It should be remarked that this could be extended, if the reaction horizon were increased beyond N_r=4, which is used in this paper. This may allow for further attempts to modify or update the constraints of the OCP in an acceptable way, similar to the steps described above. Or additional attempts may be taken to modify the algorithm or the solver settings in order to achieve an optimal, or at least a feasible solution, of the OCP.







  §.§.§ MPC-TP Driving Modes


To adequately adapt to specific traffic or environmental situations, the MPC-TP needs to introduce a switchable behavior. *) In particular, (at least)the specialized driving modes
listed in Table <ref> shall be introduced.




The proper driving mode needs to be selected by the overarching system and provided to the primary as well as the back-up MPC-TP as additional mode inputs. The MPC-TPs can then adjust their behaviors considering the detected situation. This goes hand in hand with integration requirements to detect the various driving modes.




 §.§ Refined ODD


Spec:ODDi*) To address issues, where the prediction horizon N_p of the EgoV is not large enough for the EgoV to come to a full stop if needed (cf. FI:AOUCS03 in combination with (TC:AOUCS03), we limit the maximum permissible speed in the ODD: 

    v_max=N_p a_lon,min t_s .

As the prediction horizon N_p is static,  v_max must be chosen in accordance with the maximum deceleration a_lon,min.
a_lon,min≤ 0 is the worst-case (highest) deceleration, depending on current road conditions. It depends on the worst-case values of other ODD parameters, in particular the friction value μ.  


Spec:ODDo*) To cope with (FI005) in combination with (TC015c) the ODD will be limited to freeway sections with a game fence. 

(Spec:ODDf*,h*) To mitigate (FI:Infeasible) in combination with (TC:AOUCS07), in particular with (TC:AOUCS07b), we propose to limit the minimum ambient temperature to +4^∘C  (Spec:ODDf*) and the permissible minimum road friction value (Spec:ODDh*). This will be combined with an integration requirement to conservatively estimate the friction value (IR:FrictionEstimation), cf. Section <ref>.  

The refined ODD is provided in in Table <ref>. 





 §.§ Integration Requirements


A key concept to facilitate the out-of-context development of SOTIF-related elements are assumptions on the encompassing vehicle control system. As these need to be ensured during the integration of the SOTIF-EooC into the ADS, they shall be designated as integration requirements. The new integration requirements of the SOTIF analysis are listed in Table <ref>.







Cases where neither the Primary MPC nor the Back-Up MPC is able to generate a permissible trajectory shall be handled by the encompassing system as follows:

(IR:EmergencyOperationRequest) Situations where the MPC-TP requests an emergency operation shall be handled by the encompassing system utilizing a dedicated emergency planner that is independent of the MPC-TP.

Situations involving irregular road surfaces (TC:AOUCS08) where the MPC-TP would be unable to generate a feasible (collision-free) trajectory (FI:Infeasible) shall be identified and handled by the encompassing system. Here, the Tactical Planner needs to decide what to in such cases, e.g., depending on the size of the stones or potholes and provide an appropriate tactical decision to the MPC-TP or handle the situation without the MPC-TP.
This leads to integration requirement (IR:IrregularRoadSurface): Situations involving irregular road surfaces shall be detected and handled by the encompassing system utilizing the Tactical Planner.





 §.§ Situations Requiring Further Analysis


(Further analysis required) The OCP is based on the LDBM, assuming a linear tire model and no roll motion, which may represent a poor approximation of a real vehicle if these assumptions are violated (FI:AAOD01). The non-negligible roll motion may be violated by an extremely asymmetric load (TC:AAOD01a), a highly dynamic cornering maneuver (TC:AAOD01b), or a high position of the CoG (TC:AAOD01c). A detailed simulation analysis is required to examine whether the LDBM is sufficient for the targeted use cases (especially with respect to SR:Drivable), or whether a two-track model is required instead.

Situations encompassing side wind gusts (TC:ASA01) are deemed as adequately covered by the wind limitation in the ODD (Spec:ODDg).

Potentially unknown traffic scenarios or traffic signs, e.g., due to deviating regulations in other European countries, are potentially problematic as well (TC:EAL15). However, the existing ODD restriction limits the permissible roadway types to the German Autobahn (Spec:ODDa) seems sufficient to mitigate this concern.



 §.§ Interaction-aware MPC


Particular combinations of TCs and FIs cannot be adressed without extending the architecture of the encompassing vehicle, e.g. by adding additional senors and/or capabilities. An example is the combination of (TC:AOUCS04a) and (FI:AOUCS04).
As per our analysis the OCP in (<ref>) may lead to an overly conservative behavior of the EgoV (FI:AOUCS04), possibly leading to poor progress on the road (FI:AOUCS04a), or even a deadlock (FI:AOUCS04b). The only safe way to reduce this conservatively is to internalize the relevant TgtVs in the MPC predictions and to reflect the interactions between the TgtVs and the EgoV by appropriate behavioral models. Interaction-aware MPC approaches, however, are the subject of ongoing and future research <cit.> and are beyond the scope of this paper.



§ CONCLUSION


In this paper, multiple aspects of the general SOTIF framework provided in ISO 21448 have been applied to a generic MPC-based trajectory planner, which is part of an ADS. The generic MPC-TP is treated as an SOTIF-EooC, elaborating this concept that is only sketched in the SOTIF standard. In a first step, the initial concept of a generic MPC-TP has been analyzed in order to identify relevant FIs and corresponding TCs. To this end, the analysis techniques that are described in ISO 21448 only in very general terms have to be interpreted and applied to the case study. In a second step, the resulting comprehensive list of relevant FIs and TCs informed an SOTIF-optimized concept of the MPC-TP. The revised safety architecture includes, in particular, an infeasibility analysis as a component and a back-up trajectory planner, a switchable behavior of the MPC-TP to support special driving modes, and adaptations to the reference trajectory.  

An important aspect of the results is the identification of important research topics on MPC for path planning in the context of automated and autonomous driving, from a safety perspective:


  (i) interaction-awareness with other (human) traffic participants;

  (ii) ability to avoid obstacles and drive safely even at low friction values;

  (iii) design of a recursively feasible MPC-based path planner with a reaction horizon;

  (iv) string stability in a chain of vehicles;

  (v) examination of a safe switching logic between a Primary MPC and a Backup MPC. 


In future work, an extensive simulation study would be beneficial for a more detailed understanding of the potential shortcomings of the MPC approach itself, or the respective vehicle model to be used for its predictions in extreme driving situations.



§ ACKNOWLEDGMENT


The research leading to these results was funded by the German Federal Ministry for Economic Affairs and Climate Action (BMWK) through the project EEMotion.










    10cm(4.5cm,2cm)
        

§ APPENDIX A: OVERVIEW OF TCS AND FIS

    
    [c]|p6cm|p6cm|p7cm|
    TC     Triggered FI     Notes



() traffic congestion
    
() no capability to form a rescue alley for police or ambulances() generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
    
(refined specification/design) introduce switchable behavior of the MPC (driving mode: Rescue Alley)
(integration requirement) congestion situations shall be detected and handled by the encompassing system (a,b)



() obstacle avoidance scenario 
    
() no capability to carry out emergency maneuvers without a proper lane change, driving in between two lanes, or by using the shoulder(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
    
(refined specification/design) introduce switchable behavior of the MPC (driving mode: Obstacle Avoidance)(integration requirement) obstacle avoidance situations shall be detected and handled by the encompassing system (a,b)



() blocking obstacle shortly beyond the prediction horizon N_p of the MPC-TP and no obstacle avoidance is possible 
    
() prediction horizon N_p of the MPC-TP is too short to come to a full stop within it
    
(ODD restriction) limit permissible max. speed to
v_max=|N_p a_lon,min t_s| (Spec:ODDi*)



(a) right-of-way is unclear: at the end of two merging lanes (TC:AOUCS04b) right-of-way is unclear: special situations during general lane changes 
   
(a) overly conservative behavior of the MPC-TP: inability to ensure sufficient progress on the road(FI:AOUCS04b) overly conservative behavior of the MPC-TP: inability to avoid deadlocks in certain situations (FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
    
(Interaction-aware MPC) interaction-aware MPC approach, in coordination with the Tactical Planner (Spec:InteractionAwareMPC*)



() Leading TgtV driving in reverse, in combination with a stopping or slowly moving EgoV 
    
() inability to generate a feasible (collision-free) trajectory in certain situations 
    
(refined specification/design) infeasibility analysis: selection of stopping trajectory in situations with low speed and safety distance violated



() wrong way driver 
    
(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations (FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
    
(refined specification/design) introduce switchable behavior of the MPC (driving mode: Wrong Way Driver)(integration requirement) if infeasibility still occurs, switch to emergency operation of the encompassing system ()(integration requirement) situations with wrong way drivers shall be detected and handled by dedicated mode of the encompassing system ()



(a) sudden drop of friction coefficient: water (hydroplaning) (TC:AOUCS07b) sudden drop of friction coefficient: snow / ice (TC:AOUCS07c) sudden drop of friction coefficient: oil spillage 
    
(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations 
   
(ODD restriction) limit permissible minimum temperature to exclude snowy/icy conditions (Spec:ODDf*) 
(ODD restriction) limit permissible minimum friction value (Spec:ODDh*)(integration requirement) conservative estimation of the friction value (a)(refined specification/design) avoid small areas filled with water by evasive maneuvers within the lane boundaries




(TC:AOUCS07a) sudden drop of friction coefficient: water (hydroplaning)(TC:AOUCS07b) sudden drop of friction coefficient: snow, ice (TC:AOUCS07c) sudden drop of friction coefficient: oil spillage 
    
(a) inability to generate a drivable trajectory in certain situations: due to linear tire model in LDBM
    (refined specification/design) infeasibility analysis: temporarily relax the linear tire regime if the friction value drops by more than μ_d within t_d



(a) irregular road surface: gravel or stones(TC:AOUCS08b) irregular road surface: potholes(TC:AOUCS08c) irregular road surface: road bumps
     
 (FI:Drivableb) inability to generate a drivable trajectory in certain situations: due to irregular road surface
    
(integration requirement) situations with irregular road surface shall be detected and handled by the encompassing system in combination with the Tactical Planner ()



(a) side wind gusts: bridge deck(TC:ASA02a) irregular lane markings: suddenly disappearing lane markings(b) irregular lane markings: dirty lane markings(TC:ASA02c) irregular lane markings: broken lane markings(TC:ASA02d) irregular lane markings: covered lane markings 
    
() receding-horizon planning of the MPC-TP in combination with tracking controller could lead to oscillations, or even an unstable behavior of the EgoV 
    
(refined specification/design) MPC-TP to keep the first N_r=4 steps (reaction horizon) of the trajectory unchanged (Spec:TrajectoryBodyg*) 



(TC:AOUCS04) right-of-way is unclear() other traffic participants not strictly adhering to the traffic rules 
    (FI:AOUCS04a) overly conservative behavior of the MPC-TP: inability to ensure sufficient progress on the road(FI:AOUCS04b) overly conservative behavior of the MPC-TP: inability to avoid deadlocks in certain situations(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
   
(Interaction-aware MPC) interaction-aware MPC approach, in coordination with the Tactical Planner (Spec:InteractionAwareMPC*)



26cm() cut-in of a faster TgtV in front of the EgoV
    
(FI:AOUCS04c) overly conservative behavior of the MPC-TP: overly conservative braking in cut-in scenarios 
    
(refined specification/design) tuning of MPC-TP such that it avoids overly conservative braking to a leading TgtV with a positive relative velocity

2-3
    
(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations
    
(refined specification/design) infeasibility analysis: if TgtV cuts into the EgoV's lane and is faster than EgoV, then temporarily relax linear minimum distance to this TgtV



() trailing vehicle driving closely behind the EgoV in the same lane, not keeping a proper safety distance 
    () inability of the MPC-TP to consider trailing traffic in the lane of the EgoV, where a sudden braking maneuver of the EgoV may lead to a collision 
   
(refined specification/design) MPC-TP to brake only when strictly necessary and only to an appropriate extent (Spec:SuddenBrakinga*) (refined specification/design) MPC-TP to account for distance to trailing vehicle by means of tuning the weight of ℓ_o(·,·) in the cost function (Spec:SuddenBrakingb*) 



26cm(a) suddenly appearing obstacle: previously shadowed traffic participant(TC:AFD01b) suddenly appearing obstacle: object dropping from a bridge or truck(TC:AFD01c) suddenly appearing obstacle: large animal (e.g., wild boar) crossing the freeway
    
(FI:Infeasible) inability to compute a feasible (collision-free) trajectory in certain situations () insufficient time to solve the OCP, in order to compute a new trajectory
   
(refined specification/design) infeasibility analysis(refined specification/design) potential extension of computation time to multiple time steps based on the concept of a reaction horizon

2-3
   
(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
    
(ODD restriction) limit ODD to freeway sections with a game fence (Spec:ODDo*) (refined specification/design) introduce switchable behavior of the MPC (driving mode: Defensive Driving)



(a) sudden traffic rule change: start of a speed limit area(TC:AFD02b) sudden traffic rule change: start of a no passing zone 
    
(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations(FI:InsufficientTime) insufficient time to solve the OCP, in order to compute a new trajectory 
   
(ODD restriction) limit permissible max. speed to
v_max=|N_p a_lon,min t_s| (Spec:ODDi*)(refined specification/design) infeasibility analysis(refined specification/design) potential extension of computation time to multiple time steps based on the concept of a reaction horizon




(a) LDBM assumption violation: asymmetric load(TC:AAOD01b) LDBM assumption violation: dynamic cornering(TC:AAOD01c) LDBM assumption violation: high position of the CoG 
    
()  poor approximation of real vehicle behavior by the LDBM in the OCP
    
(further analysis required) experimental analysis: evaluate if MPC-TP behaves safely in these situations; otherwise consider usage of a more complex model (e.g., a two track model) in the OCP



() chain of vehicles, EgoV in the middle, first vehicle brakes suddenly 
    
() lack of string stability of MPC-TP in a chain of vehicles, potentially leading to collisions
    
(refined specification/design) proper tuning of MPC-TP for string stability, to be verified in simulations and/or on-road tests



() fast approach of a slowly driving TgtV in the front of the EgoV, where the TgtV cannot be avoided by a lateral maneuver

   
(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations() lack of recursive feasibility 
    
(refined specification/design) infeasibility analysis(refined specification/design) if collision is unavoidable, activation of a collision mitigation mode in the encompassing system



() rapidly changing traffic situations 

   
(FI:InsufficientTime) insufficient time to solve the OCP, in order to compute a new trajectory 
   
(refined specification/design) potential extension of computation time to multiple time steps based on the concept of a reaction horizon



no specific TCs known
   
() inability to generate a solution in certain situations at all 
   
(refined specification/design) infeasibility analysis



Pairs of extreme parameter values in some or all corner cases:() max. side wind speed v_wind,max = 8 bft and max. bank angle β_max = 8^∘ () min. friction value μ_min=0.15 and min. tire cornering stiffness C_α,f,min=60 kN/rad(a) max. front tire cornering stiffness C_α,f,max=200 kN/rad × and min. rear tire cornering stiffness C_α,r,min=60 kN/rad(TC:CommonTrigger3b) min. front tire cornering stiffness C_α,f,min=60 kN/rad × and max. rear tire cornering stiffness C_α,r,max=200 kN/rad...
   
(FI:Drivablea) inability to generate a drivable trajectory in certain situations: due to linear tire model in LDBM(FI:AAOD01) poor approximation of real vehicle behavior by the LDBM in the OCP
    
(further analysis needed) experimental analysis for pairs of boundary values, conduct simulations to confirm/discard relevance of LDBM for MPC-TP; otherwise consider usage of a more complex model (e.g., a two track model) in the OCP



(TC:ASA02a) irregular lane markings: suddenly disappearing lane markings (TC:ASA02b) irregular lane markings: dirty lane markings(TC:ASA02c) irregular lane markings: broken lane markings(TC:ASA02d) irregular lane markings: covered lane markings 
    
    
(further analysis needed) common TCs, conduct simulations to confirm/discard relevance for MPC-TP 



(a) unrecognizable lane markings: direct sunlight (TC:ACTC03b) unrecognizable lane markings: change of lighting conditions, e.g., when entering a tunnel 
    
    
(further analysis needed) common TCs, conduct simulations to confirm/discard relevance for MPC-TP 



(a) insufficient visibility of lane markings: snow / ice  (TC:ACTC04b) insufficient visibility of lane markings: fog 
    
    
(further analysis needed) common TCs, conduct simulations to confirm/discard relevance for MPC-TP 



(a) infrastructure items that could be misinterpreted as lane markings: curbs (TC:ACTC05b) infrastructure items that could be misinterpreted as lane markings: dividers 
    
    
(further analysis needed) common TCs, conduct simulations to confirm/discard relevance for MPC-TP 



(TC:AOUCS08b) irregular road surface: potholes(TC:AOUCS08c) irregular road surface: road bumps 
    
    
(further analysis needed) common TCs, conduct simulations to confirm/discard relevance for MPC-TP 



(a) slippery road surface: wet road with foilage(TC:ACTC09b) slippery road surface: snowy / icy road
    
(FI:Drivablea) inability to generate a drivable trajectory in certain situations: due to linear tire model in LDBM
   
(ODD restriction) limit permissible minimum temperature to exclude snowy/icy conditions (Spec:ODDf*)(refined specification/design) infeasibility analysis: temporarily relax the linear tire regime if the friction value drops by more than μ_d within t_d(further analysis needed) common TCs, conduct simulations to confirm/discard relevance for MPC-TP 












26cm() entering the freeway from an on-ramp () exiting the freeway via an off-ramp () lane adding situations
    
(FI:InsufficientTime) insufficient time to solve the OCP, in order to compute a new trajectory(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations
    
(refined specification/design) potential extension of computation time to multiple time steps based on the concept of a reaction horizon(refined specification/design) infeasibility analysis

2-3
    
(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
    
(further analysis needed) conduct simulations and/or real experiments



26cmsituations with special vehicles or vehicles operating beyond traffic rules:(TC:AOUCS06) wrong way driver () approaching emergency vehicle () heavy transport or oversized vehicles () vehicle parking on the freeway shoulder() reckless driver / person driving under-the-influence
    
(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
    
(refined specification/design) introduce specialized driving modes for particular situations (Spec:SpecialDrivingModes*)(further analysis needed) conduct simulations and/or real experiments

2-3
    
(FI:InsufficientTime) insufficient time to solve the OCP, in order to compute a new trajectory(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations    
(refined specification/design) potential extension of computation time to multiple time steps based on the concept of a reaction horizon(refined specification/design) infeasibility analysis



26cm(TC:AOUCS08d) irregular road surface: ruts(TC:AOUCS08e) irregular road surface: blow-ups
   
(FI:InsufficientTime) insufficient time to solve the OCP, in order to compute a new trajectory(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations
    
(refined specification/design) potential extension of computation time to multiple time steps based on the concept of a reaction horizon(refined specification/design) infeasibility analysis


2-3
   
(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
    
(further analysis) extension of perception required




() freeway shoulder not available () freeway shoulder to be used as additional lane 
   
(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
   
(refined specification/design) introduce specialized driving modes for particular situations (Spec:SpecialDrivingModes*)(further analysis needed) conduct simulations and/or real experiments




() end of freeway () freeway closure
    
 (FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers
   
(further analysis needed) conduct simulations and/or real experiments

() steep road slope () high road curvature 
   
(FI:Drivablea) inability to generate a drivable trajectory in certain situations: due to linear tire model in LDBM(FI:AAOD01) poor approximation of real vehicle behavior by the LDBM in the OCP
   
(further analysis required) experimental analysis: evaluate if MPC-TP behaves safely in these situations; otherwise consider usage of a more complex model (e.g., a two track model) in the OCP



(TC:ASA01) side wind gusts
   
(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations(FI:InsufficientTime) insufficient time to solve the OCP, in order to compute a new trajectory
    
(no change needed) covered by wind limitation in the ODD (Spec:ODDg)



() potentially unknown traffic scenarios / traffic signs in other European countries
   
(FI:DangerousManeuver) generation of trajectories representing unusual, illegal, counter-intuitive, or dangerous maneuvers(FI:Infeasible) inability to generate a feasible (collision-free) trajectory in certain situations(FI:InsufficientTime) insufficient time to solve the OCP, in order to compute a new trajectory
    
(no change needed) covered by permissible road type restriction in the ODD (Spec:ODDa) 



    
    









 Paper Title: Pedestrian Inertial Navigation: An Overview of Model and Data-Driven Approaches.















Pedestrian Inertial Navigation: An Overview of Model and Data-Driven Approaches
    Itzik Klein 0000-0001-7846-0654
I. Klein heads the Autonomous Navigation and Sensor Fusion Lab, Hatter Department of Marine Technologies, Charney School of Marine Science, University of Haifa, Haifa 3498838,
Israel (e-mail: kitzik@univ@haifa.ac.il).

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

§ ABSTRACT 
The task of indoor positioning is fundamental to several applications, including navigation, healthcare, location-based services, and security. An emerging field is inertial navigation for pedestrians, which relies only on inertial sensors for positioning. In this paper, we present inertial pedestrian navigation models and learning approaches. Among these, are methods and algorithms for shoe-mounted inertial sensors and pedestrian dead reckoning (PDR) with unconstrained inertial sensors. We also address three categories of data-driven PDR strategies: activity-assisted, hybrid approaches, and learning-based frameworks.















§ INTRODUCTION

Indoor positioning is a fundamental task for various applications, including navigation, health-monitoring, location-based services, and security. Because global navigation satellite system (GNSS) signals are not available indoors,  positioning relies on other approaches such as WiFi  <cit.> or other radio frequency signals <cit.>, visual positioning <cit.>, floor plan matching <cit.>, and inertial sensing  solutions <cit.>.
Of the latter, several approaches exists, including inertial navigation system (INS), shoe-mounted INS (SM-INS), and  pedestrian dead reckoning (PDR).

For indoor navigation, low-performance inertial sensors are  commonly used, for example, those installed in smartphones and wearable devices. Therefore, without external updates, the classical INS algorithm, which requires three integrations on the measured inertial data, results in large positioning errors.  To overcome this limitation, inertial sensors can be mounted on a shoe. To limit the inertial drift, zero velocity updates and other methods of information aiding are used. But for unconstrained inertial sensors, like in smartphones, information aiding is not applicable, therefore approaches like PDR are used, which require less integrations on the inertial readings. 

The present paper is focused on SM-INS and PDR approaches, as shown in Figure <ref>. Section <ref> presents the model-based PDR framework and explains each part and its role in a common PDR algorithm. Section <ref> describes the SM-INS approach, with all relevant algorithms, including INS, nonlinear filtering, zero velocity detector, and information aiding.  Section <ref> lists three strategies for data-driven PDR, including activity-assisted, hybrid, and learning-based. Finally, Section <ref>  summarizes the paper.





§ MODEL-BASED PEDESTRIAN DEAD RECKONING

Inertial sensors, such as accelerometers and gyroscopes, are required for using model-based PDR algorithms. Smartphones or wearable devices with inertial sensors are commonly used for this purpose. An exception is shoe-mounted inertial sensors (discussed below in Section <ref>).


Model-based PDR consists of four stages (Figure <ref>):

    
  * Step detection: The pedestrian steps are detected generally based on accelerometer readings.
    
  * Step-length estimation: Several approaches may be used to estimate pedestrian step-length, including regression-based, biomechanical models, and empirical relationships.
    
  * Heading determination: The heading of the pedestrian is estimated from the gyroscope and/or magnetometer readings.
    
  * Position update: The current pedestrian position is determined based on initial conditions,  heading angle (stage 3), and step-length size (stage 2).

As with any dead-reckoning method, the user's initial position must be known before undertaking the PDR cycle. Additionally, most step-length algorithms require a calibration phase to determine their parameters. Finally, we note that most model-based PDR approaches assume that the user is walking on the same plane (floor), allowing only the estimation of the pedestrian's 2D position, excluding  altitude. 




In the following subsections, we  elaborate on each stage in the model-based PDR approach   and provide frequently used implementation methods. 



 §.§ Step detection approaches

Steps can be detected in several ways. Although some approaches have been derived for specific inertial sensor locations, with appropriate parameter tuning they can be applied also for other inertial locations.

As noted in <cit.>, step detection methods can be of four types: peak detection, zero crossing, autocorrelation, and spectral analysis.
The peak detection method detects the maximum peaks of the  accelerometer signal to determine the user step instances <cit.>. In zero crossing detection, the zero crossing of the accelerometer signal is monitored for step detection <cit.>. The cyclic nature of walking leads to strong periodicity in inertial sensor readings. It is possible to extract a cycle from a sequence of sensor data by looking for maxima in the mean-adjusted autocorrelation of the data over a period of time. Autocorrelation-based step detection can be applied to accelerometer <cit.> or gyroscopes readings <cit.>. Steps can also be detected in the frequency domain, for example, by using Fourier <cit.> or wavelet transforms <cit.>. 


Of the methods mentioned above, peak detection is the most popular. These approaches are typically based on the magnitude threshold of the specific force value and the minimum step period.  In this case, a step is defined as the interval between two successive peaks. Moreover, because PDR is a dead-reckoning approach, without external aiding, only short time scenarios can be considered.  Thus, the inertial frame (i-frame) is defined at the user’s starting point. For simplicity, we assume that the body frame (b-frame) coincides with the sensitive axes of the inertial sensors. 

To formulate the peak detection method, we denote the specific force vector expressed in the body frame, 𝐟^b_ib,  as

    𝐟^b_ib = [f_x  f_y  f_z]^T

The magnitude of the specific force vector (<ref>) at time k is

    f_mag,k = √(f_x,k^2+f_y,k^2+f_z,k^2)

The mean of the specific force magnitude throughout the trajectory is defined by

    f̅_mag = 1/n∑^n_k=1f_mag,k

where n is the number of samples. Next, The mean of the specific force magnitude (<ref>) is subtracted from the magnitude of the specific force vector (<ref>)

    f_m,k = f_mag,k - f̅_mag

Finally, the standard deviation (STD) of the specific force magnitude, after reducing its mean, is calculated by

    σ_f=  [
        1/n∑^n_k=1(f_m,k - f̅_mag)^2
     ]^1/2

 Two parameters are needed to apply a basic peak detection approach:

    
  * The minimum time interval between steps, defined empirically.
    
  * The minimum peak height, i.e., the minimum value allowed for a valid peak. It is common practice to use 1.5σ_f as the minimum value. 

We consider the following scenario to illustrate the peak detection algorithm: The user walks holding a smartphone in texting mode, that is,approximately waist-high. The user makes 26 steps to cover a distance of  25.4 meters. The specific force was measured by the smartphone accelerometers. Its magnitude, calculated by (<ref>), and magnitude mean value (<ref>) are shown in Figure <ref>. Next, the  mean is subtracted as in (<ref>) and the STD is calculated by (<ref>). Finally, the STD is multiplied by a factor of 1.5 and the resulting value is adopted as the minimum peak height. The minimum time interval between steps was set to 0.3 seconds. Using these two parameters, a simple maxima search is performed to find the user steps. 



Figure <ref> shows the specific force magnitude after subtracting its mean. The red circles show maxima values identified as steps. The peak-detection algorithm manged to detect all 26 steps. Note, however, that such approaches are sensitive to user dynamics (walking speed) and sensor location. 





 §.§ Step-length estimation

Pedestrian step-length can be estimated using several approaches, including regression-based, biomechanical models, and empirical relationships. The underlying idea behind these approaches is that a step can be detected based on  accelerometer readings. Therefore, many approaches in the literature attempt to establish a relationship between the specific force exhibited during a step and the length of the step. In a recent study, the use of smartphone inertial sensors for estimating step-length was analyzed and compared with 13 representative model-based step-length estimation models <cit.>. Lately,  <cit.> provided a survey of step-length from various perspectives, including the research method used, the length of the test path, various walking speeds, the location of the sensor device on the user's body, and the accuracy achieved in estimating step-length.


We consider three step-length approaches:

    
  * SL1: A constant step-length approach that assumes that all steps during a walk have an equal length and are a function of the user's gender and height <cit.>
    
    s_c = k_cḣ

    where s_c is the constant step-length, h is the user height, and k_c is a gain equal to 0.415 for men and 0.413 for women. As it is based solely on user height, this is one of the simplest approaches to estimating step-length. Yet, as a constant value approach, it fails to cope with varying step-lengths.
    
  * SL2: The Weinberg <cit.> is a biomechanical approach, based on the inverted pendulum model. Originally, it relied on the difference between the maximum and minimum vertical acceleration values during a step. Later, it was shown to operate successfully using  the specific force magnitude instead <cit.>. Based on the specific force magnitude, the Weinberg approach uses the formula:
      
    s_w = k_w  (f_mag,max - f_mag,min )^1/4

    where k_w is the Weinberg gain, s_w is the Weinberg-based step-length, f_mag,max is the maximum value of the specific force during the step interval, and f_mag,min is the minimum value of the specific force during the step interval. Before using the approach, a calibration procedure, with the user walking a certain distance, should be applied to determine the gain, s_w.  Taking the sum operator on both sides of (<ref>) along the trajectory, allows  determining the gain because the sum of the step-lengths is the known travelled distance. Thus, Weinberg's approach relies on a single empirical gain, which must be calibrated before its use.
    
  * SL3: Adaptive step-length estimation. The step-length of a pedestrian is not constant and varies with walking speed, step frequency, acceleration variance, and other parameters. To increase accuracy in estimating walking distances,  adaptive step-length estimation algorithms have been proposed.  Of these, in <cit.>, the step-length is a function of step frequency and the variance of the specific force during the step, as given by:
    
    s_a = k_a,1· SF + k_a,2·σ_f + k_a,3

    where SF is the step frequency defined as the inverse of the time duration of the step, σ_f is the specific force variance (<ref>), and k_a,1-k_a,3 are predefined empirical gains.  As in the Weinberg approach, the determination of the empirical gains requires a calibration procedure.  

Returning to our numeric example from Section <ref>, we tested the three step-length approaches: SL1-SL3. For SL1, the walking user was a 190 cm-tall male. Calibration procedures with the same user were carried out to obtain the gains of approaches SL2 and SL3.  The estimated distances of the three approaches are shown in Figure <ref>. Although the user walked at a constant pace to make sure that his steps were equal, the constant-length approach, SL1, performed worse than the adaptive approaches.  SL2 and SL3 obtained a distance error of less than 2% of the travelled distance. 




Figure <ref> shows the the lengths of each of the 26 steps made during the trajectory. In SL1, a constant step-length of 79cm was obtained. In both SL2 and SL3, the average step-length was 99cm, reflecting the actual step-length.
 



Step-length approaches are sensitive to user characteristics (like height and weight), inertial sensors location, and walking speeds.



 §.§ Heading and walking direction

To estimate the pedestrian's trajectory, it is necessary to estimate the  walking direction and heading. In some cases, the direction of walking does not coincide with the direction of the inertial sensor's sensitive axis.  For example, when using the smartphone, the user may point it with an offset to the  walking direction, as illustrated in Figure <ref>. 



The figure shows the heading geometry defined relative to the starting point of the trajectory. From the geometry we define:

    ψ_u = ψ_p - ψ_s

where ψ_u is the user's heading (walking direction), ψ_p is the heading angle of the smartphone (or any other inertial sensor), and ψ_s, is the sliding (offset) angle between the smartphone and the user. 
Equation (<ref>) represents a general walking scenario. To better illustrate the difference, consider the scenario shown in Figure <ref>, where a user holding a smartphone starts walking while the smartphone is aligned to the walking direction (segment A). During the walking, the user maintains the same direction but changes the smartphone direction by 45^∘, as shown in segment B. Next, simultaneously, the user changes  walking direction by 45^∘ and  smartphone direction by 90^∘,  aligning the two, as shown in segment C. Finally, in segment D, the user rotates the phone by 45^∘ relative to the  walking direction. The user trajectory and both smartphone and user heading angles are illustrated in Figure <ref>.  
  



Two spacial cases may occur: (a) the inertial sensors are held in the same direction as the walking direction, satisfying ψ_s(t)=0, ∀ t, for example, a  user walking and holding the smartphone in texting mode; (b) the inertial sensors are rigidly mounted to the user, resulting in a constant offset angle ψ_s(t)=ψ_s,c, ∀ t. 

One of the most commonly used methods for estimating the heading of smartphones, ψ_p, is based on fusing data from accelerometers, magnetometers, and gyroscopes in an attitude and heading reference system (AHRS) framework. There are many types of AHRS approaches, including the Kalman filter <cit.>, nonlinear estimation <cit.>, complementary filtering <cit.>, and hybrid-learning approaches <cit.>. Regardless of the AHRS algorithm choice, they all rely on the  kinematic equations of the gyroscope. Following  <cit.>, we provide a simplified description of these equations, assuming initial zero roll and pitch angles, and that the x-axis of the smartphone is aligned with the walking direction, resulting in an initial zero heading. This corresponds to an initial quaternion of q_0∈ℝ^4:

    q_0 = 1 + 0i + 0j + 0k

where 1, i,j,k are the quaternion basis elements.

Given the gyroscope's measurement, [ω_x ω_y ω_z]^T, the kinematic equation for the quaternion (its rate of change in time) is

    q̇ = 1/2 [ [ -q_2 -q_3 -q_4;  q_1  q_4 -q_3; -q_4  q_1  q_2;  q_3 -q_2 -q_1 ] ]  [ [ ω_x; ω_y; ω_z ] ]

Given q̇ and the previous quaternion, the current quaternion can be calculated to construct the transformation matrix from the reference  frame to the body frame:
 
    T^b_r =  [ [ q^2_1+q^2_2-q^2_3-q^2_4        2(q_2q_3-q_1q_4)        2(q_1q_3+q_2q_4);        2(q_2q_3+q_1q_4) q^2_1-q^2_2+q^2_3-q^2_4        2(q_3q_4-q_1q_2);        2(q_2q_4-q_1q_3)        2(q_1q_2+q_3q_4) q^2_1-q^2_2-q^2_3+q^2_4 ]]


Using (<ref>), the smartphone heading is calculated by

    ψ_p = atan2  ( 2(q_2q_3-q_1q_4), 1-2(q^2_3+q^2_4)   )

As noted, using (<ref>) for the estimation of the user's walking direction is valid only when the smartphone is aligned with the user's walking path so that ψ_s=0 and thus ψ_u=ψ_p. If this is not the case, other approaches to estimate  the user's walking direction are needed. 
The walking direction may be extracted from acceleration measurements using a kinematic “rolling-foot” model <cit.>, by principle component analysis of the accelerometer readings <cit.>, or using gravity-based  <cit.> and deep learning approaches <cit.>.

Here, we describe a simple yet efficient gravity-based approach for estimating the user's walking direction. First, the gravity direction vector is obtained and the angular velocity is projected to obtain its vertical component, which is integrated to find the walking direction.
As stated in <cit.>, to reduce or eliminate the large temporal variations typically associated with the acceleration of a pedestrian in motion, it is necessary to apply low-pass filtering to each axis of the force vector. Then, the gravity vector direction is given by

    γ_g = -f_LPF/||f_LPF||

where γ_g is the gravity direction vector and f_LPF is the filtered specific force vector in the sensor frame.

Denoting the vertical projection of the gyroscope reading by ω_v and the measured angular velocity by  ω, it can be calculated at any instance of time by

    ω_v = γ_g^Tω

As ω_v measures the pedestrian’s turning rate in the horizontal plane, an approximation for the change in the walking direction during the time interval Δ t is 

    Δψ_u = ω_vΔ t

The (<ref>)-(<ref>) approach for finding the change in the user's walking direction is valid assuming that the angular velocity is constant during the time interval Δ t, which holds for most gyroscopes sampling at a high-rate. Notice further that to filter out the user acceleration using a low-pass filter, the user should be walking. In transient walking motion, as at the beginning or end of walking, we expect a degradation in filtering quality, leading to large heading errors.



 §.§ Two-dimensional positioning

Given the estimated step size, s, and the user's walking direction angle, ψ_u, the two-dimensional PDR user position vector is:

    [[ x_k; y_k ] ] =
      [[ x_k-1 + s_kcos(ψ_u,k); y_k-1 + s_ksin(ψ_u,k) ] ]

where x and y are the two-dimensional position components and k is the current epoch. 

Algorithm <ref> summarizes the two-dimensional model-based PDR method.





 §.§ Three-dimensional PDR

Most PDR approaches assume that the user walks in the same plane, without vertical movement. Yet, in typical walking environments vertical movement is required, for example, when using staircases, elevators, or
escalators. A typical solution for solving the altitude problem is to use a barometer to determine the change in altitude, as applied in <cit.>. Given a barometer, the 3D PDR algorithm, for each sensor role, is presented in Figure <ref>.



Yet, as pointed out in <cit.>, not all smartphones or wearable devices have a barometer. In these cases, the altitude can be estimated using accelerometer measurements. In <cit.>, a modified Weinberg approach was suggested to estimate the user's steps during movement on a staircase and a calibration phase is required to estimate the Weinberg gain.
A later study, <cit.>, proposed estimating the change in height by identifying peaks in the motion of the user during movement on a staircase in the course of which the user is changing position. This approach does not require prior calibration.



§ SHOE-MOUNTED INS

In SM-INS (or foot-mounted) devices, the inertial sensors are rigidly mounted on a shoe<cit.>, so that zero velocity events are identified and information aiding can be applied to mitigate the inertial drift <cit.>. Therefore, instead of experiencing a velocity drift caused by a strapdown INS mechanism, a shoe-mounted INS produces a saw-tooth like velocity error behaviour. Figure <ref> illustrates this assuming ideal initial conditions and only accelerometer bias. 



In this example, every one-second stationary condition is identified and information aiding is applied to nullify velocity drift.  In practice, the same velocity error behavior is observed even when taking into account all accelerometer and gyroscope errors. 


The shoe-mounted INS framework, using an extended Kalman filter (EKF), is illustrated in Figure <ref>. Note that any other nonlinear filter can be applied instead.



The INS navigation solution is produced in accordance with the inertial conditions and inertial readings. The inertial measurements, together with the INS solution, are used in the EKF prediction phase. Next, an algorithm to detect zero velocity instances is applied. If such instances are detected, information aiding is used in the EKF update phase to produce a corrected navigation solution. Otherwise, the INS solution is not corrected by the filter.
We examine each component of the shoe-mounted INS in detail below. 



 §.§ Inertial Navigation

The INS equations of motion are a set of first order differential equations. Given initial conditions and inertial measurements, they can be solved to give the navigation solution, namely the position, velocity, and orientation. These equations are valid for any platform,  regardless of its operating environment <cit.>. In shoe-mounted INS, however, some simplification is applied, reducing the complexity of the INS equations of motion.  Generally, low-performance inertial sensors are used in shoe-mounted INS, therefore, angular velocity vector of the earth can be neglected. The transport rate is also neglected because the user velocity is relatively low. Last, it is assumed that the body frame coincides with the inertial sensor frame located on the shoe, as illustrated in Figure <ref>. 



Taking those assumptions into account, the continuous form of the INS equation of motion is:

    v̇^r = T^b_rf^b_ib + g^r

where v̇^r is the velocity vector expressed in the reference frame,  f^b_ib is the specific force vector expressed in the body frame, T^b_r is the reference to body transformation matrix defined in (<ref>), and g^r is the local gravity vector defined by:

    g^r =  [ 0, 0, g  ]^T

where g is the local gravity, assumed constant during the motion.

The position rate of change, ṗ^r,  is the integration on the velocity vector:

    ṗ^r =  v^r

Last, the transformation matrix rate of change is

    Ṫ^b_r = T^b_rω^b_ib

where ω^b_ib is the measured gyroscope angular velocity vector.

Given inertial conditions, the INS equations of motion, (<ref>), (<ref>), and (<ref>) are integrated to obtain the navigation solution. The choice of the numeric integration algorithms is a trade-off between precision and processing capability. For a detailed discussion of this topic, refer to <cit.>. In SM-INS, the Pade approximation is regularly used to propagate the transformation matrix <cit.>:

    T^b_r,k = T^b_r,k-1 ( 2I_3+Ω_kΔ t
         )
          ( 2I_3-Ω_kΔ t
         )^-1

where Δ t is the time-step, k is the current epoch, and Ω_k is the skew symmetric of the measured angular velocity

    Ω_k = 
      [ [      0 -ω_z,k  ω_y,k;  ω_z,k      0 -ω_x,k; -ω_y,k  ω_x,k      0 ] ]

The velocity is updated using the Euler method:

    v^r_k = v^r_k-1+Δ tv̇^r

where v̇^r is defined in (<ref>). Similarly, the position update is given by:

    p^r_k = p^r_k-1+Δ tṗ^r

where ṗ^r is defined in (<ref>).



 §.§ Navigation Filter

The nonlinear nature of the INS equations of motion necessitates a nonlinear filter when INS is fused with external sensors or information, as in SM-INS. Before designing the filter, we define the inertial sensor measurement error model as:

    f_ib^b    = f_ib^b + b_a + w_a 
    ω_ib^b    = ω_ib^b + b_g + w_g

where w_a and w_g are the zero mean white Gaussian noise of the accelerometer and gyroscope measurement, respectively, and the sensor biases are modeled as random walk processes

    ḃ_a      =  w_ab
    ḃ_g      =  w_gb

where b_a is the accelerometer bias,  b_g is the gyroscope bias, and w_ab and w_gb are the zero mean white Gaussian noise of the accelerometer and gyroscope bias modeling, respectively.

An error-state EKF implementation is used with an  error-state vector

    δ𝐱 = [ [ δ𝐩^r δ𝐯^r  ϵ^r  𝐛_a  𝐛_g ]]^T∈ℝ^15

where δ𝐩^r is the position error-state vector, δ𝐯^r is the velocity error-state vector, and ϵ^r is misalignment error-state. 

The linearized error-state model is <cit.>

    δ𝐱̇ = 𝐅δ𝐱  + 𝐆𝐰

where 𝐅 is the system matrix, 𝐆 is the shaping matrix, and δ𝐰 is the noise vector.
The residuals of the accelerometers and gyros  are modeled as random walk processes although any other suitable models may be used instead, such as the first-order Gauss-Markov process. The system matrix is given by

    𝐅 = [ [   0_3×3     𝐈_3 0_3 × 3  0_3× 3  0_3× 3;    𝐅_vp   0_3×3    𝐅_vϵ   𝐓_b^n  0_3× 3;   0_3×3   0_3×3   0_3×3  0_3× 3   𝐓_b^n;   0_3×3  0_3× 3  0_3× 3  0_3× 3  0_3× 3;   0_3×3  0_3× 3  0_3× 3  0_3× 3  0_3× 3 ]].

where F_ij are 3× 3  submatrices obtained from the linearization of the nonlinear equation of motion (more details on the internalization process can be found in navigation textbooks such as <cit.>). The shaping matrix is given by

    𝐆 = [ [  0_3×3 0_3× 3 0_3× 3 0_3× 3;  𝐓_b^n 0_3× 3 0_3× 3 0_3× 3;  0_3×3  𝐓_b^n 0_3× 3 0_3× 3;  0_3×3 0_3× 3    𝐈_3 0_3× 3;  0_3×3 0_3× 3 0_3× 3    𝐈_3 ]]

and the noise vector is

    𝐰 = [ [  𝐰_a  𝐰_g 𝐰_ab 𝐰_ab ]]^T


The implementation algorithm for the prediction phase of the EKF error-state closed loop is

    δ𝐱̂^-_k    =     0 
    𝐏^-_k    =    Φ_k-1𝐏^+_k-1Φ^T_k-1+𝐐_k-1

where  δ𝐱^-_k is the a priori estimate of the error-state, 𝐏^-_k is the covariance of the a priori estimation error, Φ_k is the state transition matrix, and 𝐐_k is the process noise covariance assumed to be constant throughout the trajectory.

The EKF update phase is:

    δ𝐱̂^+_k    =    𝐊_kδ𝐳_k
    𝐏^+_k    =     [𝐈-𝐊_k𝐇_k]𝐏^-_k
    𝐊_k    =    𝐏^-_k𝐇^T_k[𝐇_k𝐏^-_k𝐇^T_k+𝐑_k]^-1

where δ𝐱^+_k is the a posteriori estimate of the error-state,   𝐏^+_k is the covariance of the a posteriori estimation error, δ𝐳_k is the measurement residual vector,  𝐊_k is the Kalman gain, 𝐑_k is the measurement noise covariance assumed to be constant for all samples, and  𝐇_k is the measurement matrix.



 §.§ Zero Velocity Detectors

The function of zero velocity detectors (ZVD) in SM-INS is to determine whether information aiding can be applied at the filter update stage (<ref>)-(<ref>). In a recent review of ZVD, Wahlström and Skog <cit.> recount the history of foot-mounted inertial navigation, characterize the main sources of error, and analyze current approaches to robust ZVD. These methods include heuristic approaches, adaptive thresholding, gait cycle segmentation, other model-based approaches, as well as data-driven methods. 

There exist several heuristic ZVD that rely on different inertial features, such as the acceleration variance or magnitude, the angular rate energy detector, and the stance hypothesis optimal detection (SHOE) detector. As pointed out in <cit.>, ZVD can be formulated as a likelihood ratio check. To this end, let ℋ_0 denote a hypothesis that the inertial sensor unit is moving and ℋ_1 a hypotheses that the inertial sensor unit is stationary. Note that stationary conditions occur during walking in the stance phase or while standing still. The likelihood-ratio test based on inertial measurements, z_j, decides on hypothesis ℋ_1 if and only if

    L(z_j) = p ( z_j| ℋ_1)/p ( z_j| ℋ_0)>γ

where γ is some user-defined threshold and  z_j) = m_n^j+w_f_j-w_b with w_f and w_b as the forward and backward window length applied to the required set of measurements. 

A multi-condition bringing together both accelerometer and gyroscope measurements was proposed in <cit.>. It includes three conditions for declaring a foot to be stationary:

    
  * Acceleration magnitude. Although referred to as acceleration, this is actually a condition concerning the specific force magnitude defined by:
    
    C1 = {[                                    1 γ_fmag,min   <  f_mag,k < γ_fmag,max;                                    0                            otherwise ] .

    where f_mag,k is defined in (<ref>), and γ_fmag,min and γ_fmag,max are the minimum and maximum threshold values, respectively.
    
  * Local acceleration variance. Defines the foot activity as
    
    σ^2_f = 1/2w+1∑^j=k+w_j=k-w (f_mag,k - f̅_mag)^2

    where w is the window size and f̅_mag is defined in (<ref>). 
    The second condition is satisfied when
        
    C2 = {[           1 σ^2_f>γ_σ_f;           0   otherwise ] .

    where γ_σ_f is the local acceleration variance value.
    
  * Angular velocity magnitude. This condition requires that the angular velocity magnitude
    
    ω_mag,k = √(ω^2_x,k+ω^2_y,k+ω^2_z,k)

    must be below a given threshold γ_σ_ω
     
    C3 = {[           1 σ^2_f>γ_σ_ω;           0   otherwise ] .


Detection of a stationary foot requires that all three logical conditions be satisfied simultaneously, so a logical "AND" is applied, and the result is filtered out with a neighboring window median filter. 

Rather than use heuristic or other model-based approaches for ZVD,  several papers have recently explored the possibility of using machine learning approaches to identify zero velocity instances. For example, <cit.> used support vector machines and  <cit.> a long
short-term memory (LSTM) neural network.



 §.§ Information Aiding

Once a zero velocity instance is detected, information aiding can be applied in the EKF update phase (<ref>)-(<ref>). Mostly, two types of information aiding are used in SM-INS: (1) zero velocity update (ZVU) and (2) zero angular rate (ZAR) <cit.>.

    
  * Zero velocity update 

    The principle of this aiding states that while in stationary conditions, the velocity vector in the reference frame is zero. The ZUV measurement residual is given by:
    
    δ𝐳__ZVU   = v__INS^r - 0_3 × 1 = H__ZVUδ𝐱 + ν__ZVU

    where v__INS^r is the calculated INS velocity vector (<ref>) and ν__ZVU is zero mean white Gaussian measurement noise. The corresponding measurement matrix is defined by:
    
    H__ZVU = [ 0_3 × 3     I_3 0_3 × 3 0_3 × 3 0_3 × 3 ].

    
  * Zero angular rate 

    Similarly to ZVU,  while in stationary conditions, the angular velocity vector in the reference frame is zero. The ZAR measurement residual is given by:
    
    δ𝐳__ZAR = ω^b_ib,_INS - 0_3 × 1 = H__ZARδ𝐱 + ν__ZAR = b_g+ ν__ZAR

    where ω^b_ib,_INS is the measured INS angular rate vector, ν__ZAR is zero mean white Gaussian measurement noise,  and b_g is the gyroscope bias vector expressed in the body frame. The corresponding measurement matrix is defined by:
    
    H__ZAR = [ 0_3 × 3 0_3 × 3 0_3 × 3 0_3 × 3     I_3 ].
 



 §.§ Summary and Analytical Assessment

Algorithm <ref> summarizes the SM-INS approach. The output of the algorithm is the updated error-state vector used to correct the INS solution and inertial sensor errors. 



Next, we provide a simplified analytical assessment comparing the distance errors of the INS solution, model-based PDR, and SM-INS. For this, we assume that only an accelerometer bias of b_a=5mg is present in the system. For short time periods, the INS position error is approximated by <cit.>:

    δ p_INS = 1/2b_a t^2

where t is the time.

To assess the distance error of model-based PDR, we follow <cit.>, which proposes the MoRPI framework, a horizontal PDR-like approach for mobile robots. It was shown that the position error using the Weinberg step-length approach (<ref>) is the sum of all peak-to-peak distance errors  

    δ p_PDR = ∑^N_j=1Δ k_w Δ f_j

where N is the number of steps, Δ k_w is the error in the Weinberg gain, and Δ f_j is

    Δ f_j =  (f_mag,max - f_mag,min )^1/4

For this example, we set Δ k_w=5%.

To produce the SM-INS distance error, the error was propagated using (<ref>) during the filter prediction phase, and when zero velocity was detected, it was assumed that the filer manages to correct 90% of the existing drift. In the examined scenario, stationary conditions were detected every one second. 

Figure <ref> shows the distance error of the approaches for a duration of 30 seconds. Because of the inertial measurement errors and the dead-reckoning nature, the distance solution of all three approaches drift, although at different rates. The INS reaches a distance error of 22.5m; the model-based PDR obtains a distance error of 3m; and the SM-INS performs best, achieving a distance error of 0.75m. This error represents an improvement of 75% over the model-based PDR approach. In real world scenarios, this improvement may be higher, depending on several factors, including the walking duration.   





§ DATA-DRIVEN PEDESTRIAN DEAD RECKONING

Both machine learning and deep learning approaches learn from data, therefore they are referred to also as data-driven approaches. 
These approaches are used in a variety of navigation tasks <cit.>, including PDR <cit.>, as described below.
Data-driven PDR can be categorized into three types:

    
  * Activity-assisted PDR: Data-driven approaches are used to classify the user motion and inertial sensor location. This information is used within the model-based PDR algorithm.
    
  * Hybrid PDR approaches: The heading angle or step-length are regressed by a data-driven approach while the other by using a model-based method.
     
  * Learning-based PDR: Rather than being based on models, PDR positioning relies only on learning methods to perform the positioning task.

The above-mentioned categories as well as the model-based PDR are illustrated in Figure <ref>. In the next section, a brief introduction to neural networks (NN) provides basic concepts for those who are unfamiliar with them.





 §.§ A Brief Introduction to Neural Networks

A typical NN consist of three types of layers: input layer, holding the initial data for the entire network, output layer, producing the result of the network, and hidden layer, referring to all intermediate layers between the input and output layers. The output of the network is refereed to as the predicted value, which is compared to the true value. To this end, a loss function (cost function) is chosen to measure the difference between predicted and true values. Thus, the loss function is used as a criterion in the optimization process for obtaining undated network parameters. The optimization process is applied until the loss value is lower than a predefined threshold.  A cycle of this process is illustrated in Figure <ref>.



From a mathematical point of view, the j-th hidden layer h_j is defined as

    h_j = a ( ∑^m_i=0x_i× w_i  )

where a is a nonlinear activation function, x_i is the i, i=0… m, input to the layer, and w_i are the corresponding weights. The input layer l_in∈ℝ^n× k contains the input data of dimension n to the network, where k stands for the sequence length. For example, using the specific force vector for a sequence of 30 samples gives n=3 and k=30. The output layer, l_out∈ℝ^p, gives the output with dimension p. The complete network, n, is represented using a composition of functions, where each function is a layer in the network:    

    n = l_in∘h_1⋯∘h_f∘l_out

where h_f is the last hidden layer in the network. Note that the network performed the following mapping:

    n : ℝ^n× k→ℝ^p

A detailed explanation of NN can be found in <cit.> and <cit.>.



 §.§ Activity-Assisted PDR

One of the critical phases in model-based PDR is the step-length estimation. As addressed in Section <ref>, commonly, an empirical gain is required to estimate the step-length. This gain was found to be sensitive to the user activity mode (walking normally/slow/fast, standing, running) <cit.> and to the smartphone activity, which implies  the location of the inertial sensors <cit.>. For example, when using the smartphone sensors, the location could be texting, when writing a message, or pocket, when the smartphone is placed in the user's pocket. Each of such mode results in a different gain value for the step-length estimation phase. One solution to this problem is to take an average gain value of all expected walking mode and smartphone locations during the pedestrian trajectory. Yet, as shown in <cit.>, even an average gain value results in a 10% position error because of gain inaccuracy alone. To mitigate the influence of the human activity and sensor location, a dedicated machine learning algorithm is added to the model-based PDR stages, as illustrated in Figure <ref>, to create an activity-assisted PDR framework. 
Notice that, nevertheless, the burden of prior gain calibration is needed for all modes and sensor locations expected in the pedestrian trajectory. As soon as the classification learning algorithm identifies the current mode, the appropriate gain is selected for the PDR algorithm.



To train the learning algorithm, a data acquisition phase is conducted to collect inertial sensor data. Next, a preproccssing phase is applied, including handling missing data, normalization, noise reduction, and outlier rejection. If a machine-learning algorithm is used for classification, certain features are required. These features are extracted based on the specific force and angular velocity vector components. Feature types include statistical, time domain, cross-sensor, and frequency domain features. As the feature set is created and the required activity or location classes are defined, the training process is performed with a goal of finding the most efficient classifier for the given task. 

The same data acquisition and prepossessing phases are used with deep learning approaches but the feature extraction part is not required because the network creates its own features from the raw inertial measurements. For classification problems, a commonly used loss function is the cross entropy loss, which evaluates the difference between the predicted and true class probability distributions.  

After the classifier has been designed, during the inference phase, raw inertial data (or features) are input to the classifier to output the human activity or the inertial sensor location. An example of this cycle is illustrated in Figure <ref>, where raw inertial sensor data are plugged into a smartphone recognition network to classify the smartphone location texting, pocket, and talking. 

See  <cit.> to get a better understanding of recent advances in pedestrian-related activity recognition. Note that, as stated in <cit.>, supervised networks are trained on a set of defined user modes (smartphone locations or user activities), available during the training process. As a result, when the classifier encounters an unknown mode, it must identify it as one of the original modes that it was trained on. It is likely that such classification errors will reduce the accuracy of the PDR solution, therefore, appropriate algorithms should be applied to identify unknown modes.





 §.§ Hybrid PDR Approaches

Data-driven approaches to PDR were initially hybrid approaches. This means that they provide only one of the two quantities required for PDR. Those are the step-length and user heading. The second one is provided using a model-based PDR approach. Although recent approaches focus on a complete learning-based framework, as described in the next subsection, hybrid PDR still has utility because it requires a lighter computational load. PDR step-length estimation is of interest in healthcare and biomechanical applications. 


    
  * Step-length learning. Generally, data-driven approaches are aimed to regress the step-length estimation. They replace both the step detection and step-length estimation in a model-based procedure (Figure <ref>), so that both the heading and the user position estimation are use model-based approaches, as illustrated in Figure <ref>. In <cit.>, three StepNet architectures for the regression task are proposed. Two of them require the step detection phase and regress the step-length within the pedestrian step cycle (varying time intervals); the third, omits the step detection phase and regresses the change in distance at a predefined (constant) time interval. The last one demonstrated the best performance. Subsequently, the accuracy of step regression approaches was further improved by using magnetometer readings as well <cit.>. 
    
    
    
    A variety of neural network architectures can be used for the regression task but these are beyond the scope of this paper.  Regardless of the architecture chosen, to solve the regression problem, the mean square error (MSE) loss is commonly applied:
    
    ℒ_MSE = 1/N∑^N_i=1 (x̂_i - x_i)^2

    where N is the number of samples, x̂_i is the estimated value of sample i, and x_i is the corresponding true value. In the training process, the network is optimized by minimizing the loss function. 
    
  * Heading learning.
    Similarly, only the user heading can be regressed using data-driven approaches within the model-based PDR framework, as shown in Figure <ref>. As any other regression problem, the MSE loss (<ref>) can be used, but to better reflect the circular nature of heading, other loss functions may be considered. For example, in <cit.>, the following heading loss was suggested:
    
    ℒ_Heading = 1/N∑^N_i=1 (sin(ψ̂_i) - sin(ψ_i))^2+(cos(ψ̂_i) - cos(ψ_i))^2
 
    where ψ is the heading angle.
    
    
    




 §.§ Learning-Based PDR Frameworks

In learning-based PDR approaches, the entire model-based PDR algorithm is replaced by neural network architectures to estimate the pedestrian's position. In general, such approaches fall into two categories: 

    
  * Pedestrian position regression. Following the same line of thought as in model-based PDR, such approaches regress the change in heading and distance to estimate the user position. IONet <cit.> examined LSTM and a bidirectional LSTM architecture, whereas <cit.> used residual networks (ResNets) for the regression only after the smartphone location was classified using a different network. 

    As both the change in distance and heading are regressed simultaneously, the loss function accounts for both of them, as for example:
    
    ℒ_PDR = 1/N∑^N_i=1 (d̂_i - d_i)^2 +λ1/N∑^N_i=1 (δψ̂_i - δψ_i)^2

    where d is the distance, δψ is the change in heading, and λ is a factor to balance the two losses. 
    
  * Pedestrian velocity regression. This type of approach regresses the velocity of the user and integrates the data to determine the position of the user based on that velocity. The first work in this field, named RIDI <cit.>, regressed the user velocity and used it to correct the user acceleration. Next, double integration was applied to the corrected acceleration to estimate the pedestrian position.  By building upon RIDI, RoNIN <cit.> offers a heading-agnostic coordinate frame representing the input and output of the network. They examined three different network architectures based on ResNet, LSTM, and temporal convolutional layers. In contrast to other approaches, RoNIN uses the device orientation together with the inertial readings to provide input to the network, as opposed to using only the inertial readings.     
 



§ SUMMARY

Inertial navigation for pedestrians is an emerging discipline that has wide applications in many fields, including healthcare, security, and indoor location-based services. 
In this work, we presented model and learning approaches to inertial pedestrian navigation. We provided detailed algorithms for shoe-mounted inertial sensors and classical PDR with unconstrained inertial sensors. These algorithms include the INS equations of motion and the EKF framework aided by information in stationary conditions. For PDR, we described methods for solving each phase of the algorithm for step detection, step-length estimation, and heading determination. Following these model-based approaches, we addressed three categories of data-driven PDR: activity-assisted and hybrid approaches, and learning-based frameworks.

IEEEtran





