为纪念我的专栏第100篇内容，发布Corgi-Sim原创四足机器人仿真项目，欢迎大家基于该项目开发有趣的算法与SLAM应用！
1 项目介绍
目前四足机器人已经是国内外重要的研究热点，四足机器人的控制、状态估计算法通过仿真能快速的进行验证，目前常用的四足机器人仿真环境有MIT Cheetha自己搭建的仿真、Gazebo、Pybullet、RaiSIM等，其中后几个更是常用于强化学习的算法研究中，传统四足机器人的稳定控制主要基于力伺服控制为核心，目前国外最熟知的项目就是Mini Cheetha基于MPC的方案，基于该方案国内已经有很多公司研制出了自己的四足机器人产品，因此其是一个可以快速开展研究的仿真项目。
国内外相关的项目很多这里不再列举，本项目是一个基于Webots仿真环境的开源仿真程序，项目开源了基于VMC+QP最基本的力伺服控制方案，其与MIT早期的Balance Controller类似，因此项目中也沿用了其QP求解和状态估计的代码，项目采用的机器人对标幽灵蛛平台，为一个8自由度并联四足机器人，但是基于虚拟腿的理念也可以将仿真的算法推广至串联腿或12自由度的机器人中，本项目的软件算法源自OLDX飞控因此很多编程代码都迁移于STM32下的嵌入式版本，相比MIT的编程实现可能更利于初学者入门和国人的编程习惯，相比其他项目Corgi-Sim具有如下特点：
（1）完全基于力伺服来实现稳定控制；
（2）引入了足端力估计并融入状态机中实现着地判断与处理；
（3）引入了地形估计，可以依据地形起伏调节姿
2 新手入门
Corgi-M1配套了Webots下的仿真代码，其与实物样机的代码通用，在仿真中开发完相应算法后既可以将其复制到幽灵蛛样机工程项目中进行编译和下载，受版本同步问题部分代码需要独立移植无法完整复制，本项目的下载地址为：
链接：https://pan.baidu.com/s/1YvdGdu7AQIMrPiJgPl5ygw
提取码：2fv1注：本仿真的Webots原始模型由热心网友提供！
项目Git不定期在Littro Robotics/Corgi-M1 四足机器人仿真更新！
2.1软件中的单位进制
机器人程序中所有算法采用的单位如下：
长度：米 (m)
角度：角度（degree）
角速度：度每秒（degree/s）
力矩：牛米（N.m）
重量单位：千克 (kg)
惯性张量单位：(kg·m2)
2.2 关节与腿坐标系
Corgi由四条腿构成，其为并联腿结构，每个腿仅有两个自由度，下图为机器人俯视图，则机器人腿号定义如下：
腿序号（腿号与OD的CAN转接板的Node ID一致）：
Leg 0：FR, 右前腿
Leg 1：HR, 右后腿
Leg 2：FL, 左前腿
Leg 3：HL, 左后腿
-------------------------------------------------------------------------------
关节的序号：各腿关节坐标系一致、关节序号一致
sita0：M0电机, 机头方向的关节
sita1：M1电机, 机尾方向的关节
下图中给出了一个坐标系和关节角度示例，其中X轴为0°、M0为25°、M1为155°左右；
注：关节限位： M0关节：-45°~145° M1关节：45°~125°
如上图所示，机身坐标系下机头方向为X轴，右为Y轴，上为Z轴，各腿坐标系朝向机头为X轴，上为Z轴；
2.3 仿真机器人参数
仿真中机器人参数与Corgi-M1实物样机不一样，相应参数定义在include_vmc\base_struct.h中：
运动学参数：
小臂长度L1: 0.1m
大臂长度L2: 0.2m
末端长度L3: 0.015m
机体长度H： 0.34m
机体宽度W：0.26m
-------------------------------------------------------------------------------
物理学参数：
质量：5.0Kg
3 仿真环境的配置
仿真代码开发采用VS2017以上版本，在安装后打开项目sln工程文件前需要安装相应的矩阵和优化库仿真环境搭建需要安装VS和相应的矩阵和优化库，项目文件已经包含所有需要的源码，由于没有采用相对路径，可以先修改路径直接编译确认是否能够成功：
3.1 直接修改路径测试
（1）需要根据自己电脑上文件路径更改的目录
(原工程目录中所包含的文件夹不变，改成该文件夹在自己电脑上的路径即可,建议直接用everything搜索电脑上的文件)
配置属性—>VC++目录—>包含目录(JCQP文件目录直接删掉)；
配置属性—>VC++目录—>库目录；
配置属性—>C/C++—>常规—>附加包含目录；
配置属性—>链接器—>常规—>附加库目录；
（2）有关webots里的相关节点cpp文件(如Accelerometer.cpp)，编译显示找不到时,在解决方案管理器中点击它，查看创建工程时添加的cpp的原始路径(红框标注的完整路径)，按照该路径创建一个文件夹将webots安装路径文件夹中相关cpp拷贝到这个文件夹下即可，或者从webots的安装路径文件夹下一个个添加相应cpp
有关mpc的cpp文件直接从项目中排除，main函数里setup proble注释掉；
3.2 一步一步从头开始
（1）安装VS2017及以上版本
在之前的仿真中由于我使用的为自己STM32单片机的代码因此直接采用Webots建立C工程编程，但为使用Eigen等数学库需要转换到C++框架下，因此首先建立在系统中安装VsCoder，原因是后续许多Windows下的Cmake或gcc依赖在打开对应文件后其会自动弹出对于插件提示安装。下面首先在Webots中新建C++工程，选择VS版本并再安装VS（原因是可以直接用该软件完成许多库的路径包含，否则如选用Webots自带的gcc编译器，则需要自己修改makefile来包含需要的库十分麻烦，毕竟我们主要是为了研究算法），这里我安装的是VS2017专业版：
（秘钥KBJFW-NXHK6-W4WJM-CRMQB-G3CDH）
（2）安装Webots2021
下载并安装Webots2021并对相应的例程进行测试。
（3）配置Eigen库
在仿真文件夹中提供了默认的sln工程，下面首先安装矩阵运算所需要的Eigen库（在三方库中可以找到）。由于Eigen库均在头文件中因此，可以其解压直接放置在C盘根目录下，通过在项目属性中配置包含目录可将其快速加入工程（默认以及配置好，但需要具体查看路径是否正确）：
之后可以在Webots项目主文件中加入如下代码，点击资源管理器中显示所有文件查看是否包含Eigen目录验证：
（4）编译qpOASES库
如果你需要自己重新编译该库可以继续，否则请跳过该部分！
对于在Windows下配置qpOASES库最麻烦的一点是需要使用make等GNU链接器对下载的源码进行编译，首次编译时我遇到的问题是Window没安装该toolchain使得在qpOASES文件目录下make出错，在我安装MinGW后编译任然无果，因此我尝试成功的方法是安装Vscode后使用cmake自动产生对应makefile之后再进行编译。
当然这也是使用Vscode的好处，当你打开其对应的CMakeLists文件后Vscode会自动检查是否安装，并自动安装cmake插件，在安装完成后CD到对于目录下建立build文件夹在其中输入cmake ..则会自动产生如下配置文件：
之后可以继续make产生相应的静态库，但由于我GNU配置错误无法执行，因此需要采用VS2017打开其cmake后输出的.sln工程，使用VS2017完成对lib的生成：
cmake后的工程中有许多example例子可以直接删除仅保留需要的qpOASES相关库，之后如编译报出142故障可以在属性中修改为VS2017对于工具集：
编译后再lib文件夹下产生对应的静态库：
（5）配置qpOASES库
仿真工程已经默认添加相应的库，但是需要进一步确认你的路径，仿真程序默认放置在桌面。如果需要自己添加库首先需要在生成优化库lib后，回到Webots生成的Vscode中在属性和库目录下包含对应的头文件路径，并通过在项目浏览串口右键添加qpOASES.lib。
（6）配置项目文件目录
由于每个windows用户名不一样，正确编译可能需要修改上述目录中用户名部分与系统一致。
4 代码仿真说明
4.1 运行仿真
首先完成代码的编译，注意代码编译是webots部分需要暂停仿真并且复位
，在代码编译完成后会生成对应的exe文件：
之后在webots中运行确认其执行的exe是否是编译目录生成的文件：
如授权码正常，则机器人可正确站立和运动，如果授权码不对则无法正确进入步态：
注：为保护知识成果，限制不良盗用，本项目部分算法由于论文成果需要无法公开，目前封装了部分lib用于调用，lib中主要封装了实物样机中采用的鲁棒力跟踪控制器，其中release版本为开源采用纯力控的方案，当然性能弱于lib中的版本，采用开源版本时需要增大虚拟伺服的相关参数！
4.2 仿真启动模式的选择
由于默认情况下仿真是不介入遥控器的，为了自动实现步态切换在include_wb\include.h头文件中定义了相应步态自动切换的定义，具体实现可以参考locomotion_src\locomotion_sfm.cpp中状态机部分。
| EN_AUTO_ATT_ST | 站立启动姿态变化 |
|---|---|
| EN_AUTO_TORT | 自动切入Trot步态 |
| EN_AUTO_ATT_TROT | Trot中姿态变化 |
| EN_AUTO_PRONK | 自动切入Pronk步态（不能与其他步态复用） |
| EN_AUTO_FORWARD_TROT | Trot中自动来回运动 |
| FORWARD_REPEAT_TIME | 来回往返周期 |
| EN_AUTO_CLIMB | 自动切入Climb步态（不能与其他步态复用） |
Pronk 步态
4.3 仿真中加入遥控器
Webots仿真具有读取如PS3遥控器的接口，相应代码在locomotion_src\hardware_interface.cpp部分，仿真默认不介入遥控器，速度命令自动发送，如需要引入遥控则需要修改如下部分：
4.4 仿真数据的绘制
仿真中相应数据都存储为了txt文件，文件记录在了仿真工程Data目录下：
通过运行matlab绘图文件plot可以查看仿真中相关力控和状态反馈数据，注意每次运行时该数据会自动被刷新！数据记录代码在locomotion_src\hardware_interface.cpp，如需要增加新的数据可自行修改：
4.5 运动控制主要参数说明
Corgi采用VMC+QP或静力学分配的基本框架，其运动控制参数在locomotion_src\locomotion_sfm.cpp文件中修改，几个主要的参数说明如下：
（1）支撑和摆动的关节参数
（2）质心偏差参数
（3）VMC伺服参数
（4）力控限幅参数
（5）着地判断参数
（6）摆动相关参数
4.6 仿真代码主要文件介绍
（1）moco8_vs.cpp
该代码为webots仿真的主接口函数，其以5ms进行运行，完成仿真数据读取，融合，控制到数据输出。
（2）vmc_controller.cpp
该部分主要时VMC伺服控制器：
以及虚拟伺服控制量分配接口：
（3）state_estimator.cpp
该部分主要是着地判断：
足底力估计：
步态中质心状态估计：
以及地形角度估计：
（4）leg_planner1.cpp
该部分主要是摆动轨迹的规划，首先是落足点选择：
之后是摆动控制和规划：
（5）math_src
该文件中包括运动学、雅可比、轨迹规划等基础的运算库：
（6）Gait_src
该文件中包括trot、stance等步态的状态机和规划算法：
评论（0）
您还未登录，请登录后发表或查看评论