# Leg Phase detection

**Author:** Teng-Hu Cheng
**email:** [e-mail]
**Date:** Jan. 2025
**Website:** https://ncrl.lab.nycu.edu.tw/member/

[toc]

# 摘要
- 由於大部分機器狗為了節省成本，會省去安裝足底壓力感測器，而此感測器是判斷該支腳是否落地的重要元件，因此，必須向替代方案才能移除該感測元件。該腳落地的狀態判斷相當重要，會影響機器狗stance/swing腳狀態的切換，且影響whole body control的穩定性，因為該演算法假設stance是使用torque control來穩定機器狗。
- 這邊我們透過四足機器人的狀態，包含身體角速度、身體加速度、12軸馬達力矩、足底位置誤差(FK)，並且標注是足底否觸地的狀態，來建立機率模型。這些資料都要在走路實驗過程記錄，這樣我可以拿這些標注的資料算觸地的機率模型。

- 方法使用的是EKF+Bayesian filter。

- 步驟：收集資料，微調機率模型(四足機器人每個狀態的mean、covariance)

# **Sensor Fusion and Probabilistic Methods in Quadruped Robot Gait Phase Switching**

Quadruped robots rely on **sensor fusion** and **probabilistic models** to accurately determine the switching between **swing** and **stance** phases for each leg. Since real-world terrains and robot dynamics are often uncertain, **robust detection and prediction of gait phases** are critical for **stable locomotion, adaptive terrain response, and efficient movement**.

---

## **1. Swing and Stance Phase Detection**
Each leg of a quadruped robot alternates between two key phases:
- **Stance phase**: The foot is in contact with the ground, supporting the robot’s weight.
- **Swing phase**: The foot is lifted and moves forward to the next step.

### **Challenges in Phase Switching:**
1. **Uneven terrain**: Different terrains introduce variations in expected ground reaction forces (GRF).
2. **Foot slippage**: Detecting false contact (e.g., foot dragging or slipping).
3. **External disturbances**: Unexpected forces (wind, impacts) can alter the expected gait pattern.
4. **Sensor noise and drift**: Individual sensors are prone to measurement errors, making sensor fusion essential.

---

## **2. Sensor Fusion for Gait Phase Estimation**
To determine the current phase, quadruped robots fuse data from multiple sensors:

### ~~**(1) Ground Reaction Force (GRF) Sensors**~~
- Measures **contact force** at the foot.
- If **GRF > threshold**, the leg is likely in **stance**; otherwise, it's in **swing**.
- Useful but **can fail on soft surfaces** (e.g., sand, grass).

### ~~**(2) IMU (Inertial Measurement Unit)**~~
- Measures **angular velocity (gyroscope) and acceleration (accelerometer)**.
- Helps detect foot **impacts, velocity changes, and slip events**.
- Used to estimate **body acceleration and tilt**, which correlates with phase transition.

### **(3) FK (Forward Kinematics)**
- Measure the position error of the foot position.
- **Higher foot position error → likely stance phase**.
### **(4) Joint Encoders & Motor Current Feedback**
- Measures **joint angles (e.g., FK) and torques**.
- Detects if the leg is moving (swing) or resisting external forces (stance).
- **Higher torque → likely stance phase**.

### **(5) Vision & Depth Sensors (Optional)**
- LiDAR, stereo cameras, or depth sensors estimate the **terrain height**.
- Helps anticipate foot placement and predict **when the leg should transition** to stance.

### **Sensor Fusion Process:**
Sensor fusion combines the above sources to **filter out noise and improve phase estimation** using:
1. **Extended Kalman Filter (EKF)**
- Fuses IMU + GRF + joint encoders.
- Estimates leg states (stance/swing) probabilistically.

2. **Bayesian Inference**
- Uses probability models to infer stance/swing likelihood based on noisy sensor data.

3. **Machine Learning / Neural Networks**
- Uses **historical gait data** to predict **optimal switching points**.

---

## **3. Probabilistic Models for Phase Switching**
Since sensors are noisy and terrain conditions vary, **probabilistic models** help predict the correct phase transition.

### **(1) Hidden Markov Model (HMM) for Phase Transitions**
- The **stance/swing phase** is treated as a **hidden state** $S_t$.
- Observations $O_t$ come from fused sensor data.
- The transition probability $P(S_t | S_{t-1})$ depends on:
- Foot velocity.
- Contact force.
- Terrain estimation.

**HMM State Transition Matrix Example**:
$$P(S_t | S_{t-1}) =
\begin{bmatrix}
P(\text{stance}|\text{stance}) & P(\text{swing}|\text{stance}) \\
P(\text{stance}|\text{swing}) & P(\text{swing}|\text{swing})
\end{bmatrix}$$

### **(2) Gaussian Mixture Models (GMM)**
- **Clusters gait sensor data** into **stance** and **swing** phases.
- Uses **likelihood estimation** to classify the current phase.

### **(3) Reinforcement Learning for Gait Adaptation**
- **State $s_t$ → Sensor measurements.**
- **Action $a_t$ → Decide swing/stance transition.**
- The model **learns optimal transitions** from real-world terrain variations.

---

## **4. Example: Bayesian Decision Rule for Phase Switching**
To determine whether a foot should switch to stance or swing, we compute:

$$P(\text{stance} | O_t) = \frac{P(O_t | \text{stance}) P(\text{stance})}{P(O_t)}$$

Where:
- $P(O_t | \text{stance})$ is the **likelihood** of observing current sensor data given stance phase.
- $P(\text{stance})$ is the **prior probability** of stance (e.g., if robot is slow, stance probability is higher).
- $P(O_t)$ is the **normalization factor**.

**Decision Rule:**
- If $P(\text{stance} | O_t) > 0.5$, switch to stance.
- Else, stay in swing phase.

**Conclusion**
- Input: the observation, Prior Probability
- Output: the Probability
---

## **5. Implementation in MATLAB/Python**
Here’s a MATLAB example using Extended Kalman Filter (EKF) based on FK、Joint Torque for gait estimation:

function phase = gait_phase_estimator(GRF, IMU, JointTorque)
    % Define prior probabilities

    
    % Likelihood estimation
    L_stance = normpdf(FK_error, 0.05, 0.05) * normpdf(JointTorque, 5, 2);
    L_swing = normpdf(FK_error, 0, 0.01) * normpdf(JointTorque, 2, 1);

    % Compute posterior probabilities
    P_stance_given_O = (L_stance * P_stance) / (L_stance + L_swing);
    P_swing_given_O = (L_swing * P_swing) / (L_stance + L_swing);

    % Decision rule
    if P_stance_given_O > P_swing_given_O
        phase = 'Stance';
    else
        phase = 'Swing';
    end
    P_stance=P_stance_given_O/(P_stance_given_O+P_swing_given_O);
    P_swing=1-P_stance;
end

% Example usage
P_stance = 0.6; 
P_swing = 0.4;
%GRF = 85; 
%IMU = 0.1; 
JointTorque = 6;
FK_error=0.03;
phase = gait_phase_estimator(GRF, IMU, JointTorque);
disp(['Predicted phase: ', phase]);

Here’s a **MATLAB example** using **Extended Kalman Filter (EKF) for gait estimation**:

function phase = gait_phase_estimator(GRF, IMU, JointTorque)
    % Define prior probabilities
    P_stance = 0.6; 
    P_swing = 0.4;
    
    % Likelihood estimation
    L_stance = normpdf(GRF, 80, 15) * normpdf(IMU, 0, 0.1) * normpdf(JointTorque, 5, 2);
    L_swing = normpdf(GRF, 20, 10) * normpdf(IMU, 2, 0.5) * normpdf(JointTorque, 2, 1);

    % Compute posterior probabilities
    P_stance_given_O = (L_stance * P_stance) / (L_stance + L_swing);
    P_swing_given_O = (L_swing * P_swing) / (L_stance + L_swing);

    % Decision rule
    if P_stance_given_O > P_swing_given_O
        phase = 'Stance';
    else
        phase = 'Swing';
    end
end

% Example usage
GRF = 85; IMU = 0.1; JointTorque = 6;
phase = gait_phase_estimator(GRF, IMU, JointTorque);
disp(['Predicted phase: ', phase]);

---

## **6. Summary**
✅ **Sensor Fusion:** Combines **GRF, IMU, Joint Sensors, and Vision** for accurate phase detection.
✅ **Probability Models:** Uses **HMM, Bayesian Inference, and GMM** to infer gait transitions.
✅ **Adaptive Control:** Machine Learning or Reinforcement Learning helps adjust gait **based on terrain**.
✅ **Practical Use:** Implemented in **Boston Dynamics' Spot, ANYmal, and MIT Cheetah** robots.

With **sensor fusion and probabilistic models**, quadruped robots achieve **adaptive, stable, and efficient gait control** on complex terrains. 🚀🐾