# gravity_wheel_MPC

**Repository Path**: Anthony_Bridge/gravity_wheel_MPC

## Basic Information

- **Project Name**: gravity_wheel_MPC
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: GPL-3.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-03-19
- **Last Updated**: 2025-03-19

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Mujoco 倒立摆 基于学习的MPC控制


## 理论
### 1. ​问题定义与符号
**状态空间**：$s_{t} =\left [ \theta _{t}, \dot{\theta }_{t}  \right ]$，其中$\theta _{t}$为角度，$\dot{\theta }_{t}$为角速度<br>
**控制输入**：$ u_{t} \in \mathbb{R} $，表示施加的力矩，正负代表方向<br>
**目标函数**：最小化未来$ N $步的累积代价： 
$$
J = \sum_{k=0}^{N-1}\left ( 
\left \|  \theta _{t+k|t} - \theta _{ref}  \right \|^{2}   
+\lambda \left \| u_{t+k}  \right \| ^{2} 
\right )
$$
其中$\theta _{ref}$为平衡位置（如垂直向上$0^{\circ }$），$\lambda$为控制量权重

### 2. 神经网络动态模型
**输入**: 当前状态$s_{t}$和动作$u_{t}$<br>
**输出**: 预测下一状态$\hat{s} _{t+1} =  f_{\theta } \left ( s_{t}, u_{t}  \right ) $，其中$f_{\theta } $为神经网络参数化模型<br>
**训练目标**: 最小化预测误差： 
$$\mathcal{L} \left ( \theta  \right ) =\mathbb{E} \left ( s_{t},u_{t},s_{t+1}    \right ) \left [ \left \| \hat{s}_{t+1}-s_{t+1}    \right \|^{2}   \right ] $$

### 3. 滚动优化
**当前状态获取**:$s_{t}$从仿真环境读取<br>
**控制序列优化**:
$$\underset{u_{t},\dots , u_{t+N-1}}{min} 
\sum_{k=0}^{N-1}
\left (\left (  \left | \theta _{k+1} \right | + 0.1*\left | \dot{\theta }_{k+1}  \right | 
\right ) * \left ( 1 - step / 10 \right )\right )
$$
约束条件为$\hat{s} _{t+k+k|t} = f_{\theta }\left ( \hat{s}_{t+k|t},u_{t+k}  \right ) $（多步预测）<br>
**实施控制**: 取优化序列首项$u_{t}^{*}$作用于系统<br>
**在线更新模型**(如果动态更新): 将新数据$\left ( s_{t},u_{t}^{*},s_{t+1} \right )$ 加入训练集，更新$f_{\theta}$



## python库依赖
```
mujoco
torch
```

## 开始
### 在线训练
```
python model_train_online.py
```
训练后会在train_records文件夹中保存模型<br>
但是在线训练效果感觉不太好，所以建议离线训练（原因不明，于是我使用了离线训练）

### 离线训练
#### 1. 收集预训练数据
```
python data_collection/data_collection.py
```
每个程序会收集2.5e4 * 21 条数据，可以多次运行这个程序收集更多的数据

#### 2. 数据合并
```
python data_collection/data_merge.py
```
如果在收集数据的时候手机了多个json的数据，可以使用这个程序合并成一个json，合并后的文件命名为"data_merge.json"，但是这个文件比较大，不要在vscode或cursor中打开，大概率会卡死！！！

#### 3. 训练模型
```
python model_train.py
```
训练完成后的模型命名为"robot_dynamics_model"开头

### 测试模型
```
python test.py
```
注意在主函数中修改pth模型路径
<div style="text-align:center">
  <img src="imgs/1.png" alt="图片描述" style="width: 85%; height: auto;"/>
</div>