# wheel_leg_LB_MPC

**Repository Path**: Anthony_Bridge/wheel_leg_lb_mpc

## Basic Information

- **Project Name**: wheel_leg_LB_MPC
- **Description**: 双腿轮足机器人，基于learning的MPC控制研究
- **Primary Language**: Unknown
- **License**: GPL-3.0
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 0
- **Created**: 2025-03-27
- **Last Updated**: 2025-04-09

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 双腿轮足机器人，基于learning的MPC控制研究


这个仓库是是[Mujoco 倒立摆 基于学习的MPC控制](https://gitee.com/Anthony_Bridge/gravity_wheel_MPC)的延伸，将训练和控制流程移植到了一个双腿轮足的机器人上。<br>
由于个人时间、能力、精力等各方面限制，该机器人的控制还有一些问题解决，例如机器人还不能实现转向，只能直线运动，并且在静止和前进的时候，机器人会陷入一定程度的震荡，虽然能够保持站立，但是依然谈不上稳定。现在就先挖个孔吧，以后再慢慢填。<br>

<div style="text-align:center">
  <img src="images/1.png" style="width: 85%; height: auto;"/>
</div>

## 基本控制方案
这个双腿轮足机器人一共有6个关节，可以在"model/robot.xml"的模型文件中看到，也可以在"mj/config.py"中看到，左右两条腿分别有thigh、calf、wheel三个关节，其中thigh和calf是连杆，wheel是轮子，每个关节都有位置和速度两个传感器，另外还有一个IMU传感器，用于获取机器人的姿态信息。<br>
thigh、calf一共四个关节，采用PID控制方案，控制器可以在"mj/mj.py"的PIDController类中看到；wheel一共两个关节，采用MPC控制方案，采用离散动作控制。<br>


## 基于深度学习的模型预测控制
在一般的MPC控制中，模型预测控制需要建立系统的动力学模型，然后通过优化算法求解最优控制序列，但是在多输入多输出的机器人控制中，往往难以用数学去建立一个准确的状态预测模型，但是如果在仿真环境中能够相对准确地模拟机器人的运动状态，则可以通过一个神经网络拟合机器人的数学模型，并用于状态预测。<br>
我建立了如下的神经网络：
```
        self.net = nn.Sequential(
            nn.Linear(state_dim + action_dim, 256),
            nn.ReLU(),
            nn.Linear(256, 256),
            nn.ReLU(),
            nn.Linear(256, state_dim)
        )
```
state_dim一共8维，见于"mj/mj.py"Env类的get_state函；action_dim一共2维，代表左右轮的力矩。<br>
经过实验，从经验上来看，采用在线训练的方式"model_train_online.py"效果不理想(反正我训不出来:sob:)，而采用预先收集数据，再离线训练的方式效果好很多，训练效果也比较可控。<br>
所以下面的内容都是基于离线训练的。

## 仿真环境介绍
MJCF文件定义在了"model/robot.xml"，相关的内容就不赘述了。<br>
主要说一下"mj/mj.py"中的Env类，这个类是仿真的核心。<br>
该类的初始化需要传入xml文件的路径和是否需要启动渲染"enable_render"，如果enable_render为True，则会自动执行self.launch_viewer()启动渲染，否则就只模拟而不渲染仿真环境图像。不够，即使enable_render在一开始设置为了False，也可以在程序运行中手动执行self.launch_viewer()拉起渲染（例如在训练过程中，需要临时启动渲染以查看训练效果），注意启动渲染后，在不需要渲染后使用self.close_viewer()关闭渲染。<br>

### 重置环境
通过Env类的reset函数重置环境，重置后，机器人会回到初始状态，回到原点。在reset函数可以接收两个参数传入，分别是欧拉角和轮速，如果传入这两个参数，将会在初始化的时候进行设置。

### 获取机器人状态
通过Env类的get_state获取state，其中state包括有8个维度，依次为xy欧拉角[2]，左右轮速度[2]，线速度(体轴系前进速度)[1]，imu角速度(gyro)[3]，这里的线速度是机器人体轴系下的速度，为一维标量，正负代表前进和后退。

### 推进环境
仿真推进一步使用Env类中的step函数，需要传入一个action，这个action是一个二维的list，代表左右轮的力矩。<br>
在step函数中，程序会自动执行thigh、calf关节的PID控制，自动计算这四个关节的力矩。<br>
thigh、calf关节和wheel关节的力矩拼接后，通过self.set_torque()函数设置。<br>
为了使得每次动作的反应在机器人的状态变化上的程度更大，在每一次step中会对相同动作进行循环推进10次，最后返回执行后状态state和是否结束的表示done，如果机器人在仿真中x或y轴的欧拉角偏离0点过大，即机器人倒下了，这种情况下的数据没有参考意义，因此此时done为True，表示应该结束重新开始了。

### 收集数据
通过"data_collection/collect_data.py"进行数据收集，其实收集数据的过程实际上也是进行了一次在线训练，这是为了让收集的数据更有意义，如果数据完全是随机执行的，将会有大量的没有意义的数据出现，因此一边收集数据一边在线训练迭代一个神经网络，使得收集到的数据更加贴近我们期望的机器人的运行状态。<br>
每执行一次数据收集，可以获得DATA_NUM条数据，这些数据将会以多个json文件的格式保存在"data_collection"文件夹中。<br>
为了加快数据收集，尤其是在有多核心工作站的时候，可以使用多个终端同时运行多个"data_collection/collect_data.py"以同时收集数据。如果系统环境是ubuntu20或22，可以修改并执行run_data_collect.sh脚本自动启动多线程数据收集。注意根据实际修改COMMANDS中的命令和打开终端的数量。<br>
数据收集完毕后，使用"data_collection/data_merge.py"程序，将多个json文件合并为一个json文件，合并后的文件为"data_collection/data_merge.json"，到此数据集就准备完毕了。

### 训练模型
最好在一台有torch GPU加速的电脑上进行训练，使用"model_train.py"进行训练，训练会自动使用"data_collection/data_merge.json"数据进行训练，循环后的模型会如"robot_dynamics_model-2025-03-26-20-21-04.pth"这样保存。训练过程会打印train_loss和valid_loss，只不过没有保存，想要保存就自己写一个保存的函数。

### 测试模型
使用"test.py"进行测试，注意修改：
```
load_path = "robot_dynamics_model-2025-03-26-20-21-04.pth"
```
为实际的模型。运行就可以看到效果了。