wheelfoot code

机械

电机选型

1. 驱动轮电机

   • 最好是直驱电机，可以减少背隙和阻力，但是有减速箱也不是不行，但是尽量保证减速箱的背隙与阻力比较小
   • 力矩线性度要好，就是再建模和控制方面都是作为力矩器来使用的，也就是再建模过程中输入的是一个力矩
   • 低速状态下力矩输出稳定
   • 功率上限高

2. 关节电机

   • 通信稳定
   • 散热性好
   • 峰值输出力矩比较大（主要是起跳和落地）
   • 减速箱的背隙和阻力不需考虑

腿部连杆长度选定

初选定杆长参数

杆	长度
l1	0.16
l2	0.32
l3	0.32
l4	0.16
l5	0.20

实际选定杆长

杆	长度
l1	0.18
l2	0.336
l3	0.336
l4	0.18
l5	0.24

通讯

can通信

• can1 作为关节电机 Tmotor 的通讯
• can2 作为足端轮子电机 DJmotor 的通讯

usart通信

• usart1 作为通讯的接口，使用蓝牙通讯，与主控控制器通讯
• usart2 作为传感器的通讯接口，用于传感器的通讯

规定

0-1	2	3-n+3	n+4-n+5
head	id	data	tail

• head: 0xff 0xfe
• id:
  • 1 控制参数 ControlParam
  • 2 摇杆数据 HandleParam
  • 3 机器人状态参数 RobotState
  • 4
• data: 利用memcpy来实现复制
• tail: 0x0a 0x0d

控制

腿部的控制

设定从右侧看机器人为正视角，并且以向右为机器人正方向

所以右方向的腿被认为是正方向的腿，右侧的腿与右侧的腿完全对称就可以直接反过来了，所以只需要一个方向就可以直接规定所有不一致的方向了，所以对于设置电机的力矩之类的，都需要乘以一个方向值了

规定

• angle1为前方电机的角度
• angle4为后方电机的角度
• TF 即为前部电机的力矩，对应于angle1
• TB 即为前部电机的力矩，对应于angle4
• Tb 为关节处扭矩，是一个虚拟出来的力，定义逆时针为正
• 一般会把 angle1 初始化为180度，也就是在angle=180的地方作为零点，同样以 angle4 初始化为0度

电机控制

电机的正方向是可以人为改变的，我这里打算把电机的正方向设置成一致的，也就是正向轴端逆时针为正（但是实际上我做成了正向轴端顺时针为正），这样对于解算来说，角度的正方向与电机的正方向一致，直接位控的话很方便，但是肯定不做位控()。电机最后只需要腿部的方向作为控制的方向了，对于需要反向的电机，需要在初始化的时候就设置反向

实际上，由于条件限制，电机正方向是逆时针为负，所以对于右侧电机，角度变化是负的，对于左侧电机，角度变化就是正的了

• DJmotor: 因为是通过PID来对输入进行解算的，所以只需要把输入做一个反向就好了，后面的输出都会是反向的

  id号规定

  • 0 右侧轮子电机
  • 1 左侧轮子电机

• Tmotor: 数据直接作用于电机，所以输出的正负直接影响电机的表现

  id号规定

  • 左前 1
  • 左后 2
  • 右前 3
  • 右后 4

机体控制

• lqr 控制器，很经典，但是控制上也会有很多缺陷，例如响应不够迅速，而且有些情况下就不适合使用了(腾空状态)，但是可以腾空之后只控制腿部的位姿
• 腾空检测 对地支持力的检测
• 机器人初始化的时候腿部需要一个限位块来确定电机的初始位置
• adrc 之后研究一下

跳跃控制

分相位来控制，相当于一个跳跃分为多个阶段来实现，依靠设定 L0pid.target 来实现跳跃

• 跳跃准备阶段——收缩
• 跳跃1阶段——蹬腿 kick
• 跳跃2阶段——收腿 shrink
• 空中姿态LQR平衡——依靠lqr来保持机体腿部竖直
• 缓冲——依靠腿长PID做一个阻尼控制来缓冲

参考

• RoboMaster平衡步兵机器人控制系统设计
• RoboMaster平衡步兵机器人控制系统设计视频
• Balance Control of a Novel Wheel-legged Robot: Design and Experiments
• A Two-Wheeled Jumping Robot

注意事项

C语言结构体中数据的对齐

代码风格

• 函数 大写开头驼峰式
• 宏定义 纯大写+下划线
• 内部函数 小写开头驼峰式
• 全局变量 小写+下划线
• 局部变量 下划线开头的小写+下划线

对齐方式

会以结构体中最大的数据类型来进行对齐，会造成内存的浪费

规定

• 使用 #pragma pack(n) 来进行定义，但是会导致使用内存读取速度降低
• 或者定义的时候要注意，将小的数据类型放在前面

踩过的坑——希望看到的人别踩到了😱

代码的优化等级需要注意

• 优化等级一般选用 -O0，如果使用较高的优化等级 (-O2) 会导致调试的时候出问题，而且代码的执行顺序也不一定按照自己的需求来，也就是说代码怎么运行的只有上帝才知道了。。。。
• 优化等级可能会导致板子下不进去代码，一定记得改好优化等级，建议 -O0，你要是nb也可以试试 -O2，毕竟可以优化代码运行效率，但是调试时你就知道痛苦了（变量已被优化）

代码上的一些问题

• can消息定义一定不要忘记，要把所有该定义的的全部定义完成，有可能导致CAN消息的id号错误和消息错误(这很致命)
• 对于pid的计算，一定要注意，每次运行只进行一次运算，多次运算会导致一些问题
• M3508电机控制频率问题，频率太低控制不是很好
• 对于电机该使用的参数一定要写对，还有三角函数的计算使用的是弧度制
• 对于一些状态量的更新，一次运行只做一次就行，多写会出问题，会造成一些较大的抖动
• 对于腿长的控制，需要两条腿分别控制，一定不能用平均值
• 代码运行频率要做好，也可以做一些状态的预估值来做辅助解算
• 代码中对于 arctan 量的解算要使用 atan2f，由于 atan 中形参分母为0是会直接跑死的，但是 atan2f 与 atan 的解算结果的范围是不一样的，这一点需要注意

代码运行速率的优化

• 注意尽量写代码时规划好，别在同一时刻重复去做一件事
• 对于整数的乘法，尽量使用移位运算代替，减少运行消耗的时间
• 尽量别使用 if-else if-else，使用 switch
• 尽量使用移位操作来代替乘法，除法（仅限于整数）

控制上的一些问题

• 如果解算频率太快会导致收不到电机反馈而无法做出正确的解算，特别是力控这样，会导致很严重的抖动（滞后控制）
• 要注意所有参数的方向和大小，一些基本参数一定不要给错，对于所有参数方向，在解算中会写明白的

README

写代码前提前规划好所有要写的和一些代码中需要的规定是个人习惯，并且在写代码的时候严格按照readme里面规定的架构来写，主要还是因为本人比较菜，又需要保证代码的结构和质量 T_T

这套代码里有之前写的模板类，所以感觉还是挺不错的，但是就是很难去debug，所以我是提前把功能调好了之后才把这段代码复制过来的，但是使用的时候要注意，并且记得在头文件里声明需要露出来的接口（感觉这点又有点繁琐，但是能够隐藏接口还是感觉挺不错的）。并且写这些模板类或者模板函数很好的一点就是能够减少很多重复的实现，使代码看起来更简洁（前提是写的没有bug）

对于这个系统，一定要在仿真里多跑一跑，把需要上车的代码多去验证一下，会发现并且解决很多在实物上不容易发现并且解决的问题

代码更新中.....

调试PLAN

基础功能

• 腿长PID
• splitpid
• 下地平衡
• rollpid 对于这个pid，总感觉控制量应当是腿长，而不是力矩呢，因为如果是力矩就会和腿长PID叠加，然后导致机体roll并不能适应环境
• yawpid
• WBC控制器

进阶功能

• 离地检测
• 跳跃

调试寄录

调试流程

L0PID->splitpid->下地调节平衡->rollpid->yawpid

12.4

第一次上车，连腿子都没搞好，寄😭

12.5

下午调试，很有问题，还是之前的问题，还好我坚定代码流程没问题，最后是电机的底层代码写的有问题，蓝瘦。。。。。。改完就没问题了，就剩下调节PID了，nice! 一定要经常使用示波器来查看状态，很有用

12.6

调试PID有了一点成果，但是也发现系统有个非常致命的问题，就是当angle4角度>90°时，解算出的力矩竟然是向内的，这会加剧这种影响

控制腿长的PID终于调好了，总结一下调节PID的流程

• 先调P，有些系统里只给P的话会有震荡，主要还是系统本身自带的D和一些惯性的影响，有点震荡很正常，可以适当的给到超调（如果系统允许的话）
• 再调D，给一个比较好的阻尼系数，让系统减少震荡次数，可以不用给太大，有点震荡还好，特别是对于腿长的控制，要做缓冲，要有点弹簧系统的感觉
• 再调I：由于只有PD的话，会有稳态误差，给一个I可以减小稳态误差，要注意I是累积量，影响会很大，可以适当给小一点ki，如果出现高频震荡，可以适当减小一点I，或者增大一点D
• 调节这些系数都是要从小开始，一下给太会造成一些意料之外的状况

12.11

轮足对腿长的PID控制应当是两条腿分开控制的，但是我直接使用平均值，属于是纯纯的傻蛋了T_T

调节split_pid

12.13

把代码放进仿真里跑了跑，发现了抖动的问题，太剧烈了，研究了一上午才发现，原来是状态量更新做了两次 T_T，受不鸟了....改了之后发现在仿真里跑的很好，看来代码整体上是没问题的.

解决了腾空检测的一点点小问题，关于虚拟力方向的问题

12.19

调试过程中经常出现抖动，我怀疑是机器人的零点的问题，就是所认为的零点并不是真实的零点，再加上腿比较重，所以当腿部 theta=0 时， Tb=0，但是机器人腿并不在一个平衡的位置，也就是这时候所需的 Tb 并不为0，导致抖动。

问题发现原因：这个代码在仿真里做的很好，但是在实物上很差，而且之前的调试中发现腿部所平衡的位置并不位于机器人的正下方

12.21

发现抖动的原因，应该是电机反馈跟不上解算的速度吧，但是实际上解算是10ms一次，按理说电机应该能够接到反馈的，感觉很离谱

12.22

把代码移植到我的仿真里面之后，成功的运行了，也就改了反馈频率的问题，看来对于力控来说，及时反馈是十分重要的

12.23

解决了抖动的问题，原来是电机的控制频率的问题？反正我把每次发送can消息数量变成了原来的两倍，也就是一次发送两条消息，然后就解决了？？我认为应该是电机反馈频率没跟上，也就是没来得及更新电机状态，导致的滞后控制

抖动问题解决之后一切进度就可以加速进行了😊

oh!!!!!! 终于两条腿都稳定了，看来明天下地有望！！！！

12.24

又出现了抖动的问题，终于找到了原因，原来是控制频率太低了，把控制频率改高就好了，基本上解决了抖动的问题

md，站不起来，令人痛苦，在仿真中 l0pid 和 splitpid 调节好之后就能成功站起来了，但是实物却没站起来，一时间竟不知该怎么搞了，蓝瘦

12.26

可能是电机减速比设置的有问题吧，明天测试

3.8

时隔两个多月，又开始调试了，发现了之前没调好的一些问题，主要是各个解算的正方向问题，重新把代码整理了一番，还是之前对整个系统的理解不够深刻啊

3.11

时隔好久，终于成功了，也总结了一些经验，对于腿长PID的调节，先在架子上调好的PID并不能完全使用，下地所需要的PID的D一定要比架子上的D大的多，不然会导致超调并且不到位，站不起来，这个调好之后基本上就没啥问题了

3.12

基本上解决了所有问题，原来是因为轮子半径给错了，这个一定要注意，太致命了 第二代轮腿的调试只用了4天就成功了，总结一下经验

1. 首先肯定是经验不足，第一代轮腿做测试的时候，基本上是用的学长写的代码，自己的思考也不是很清楚，被电机和各个参数量的正方向所困扰，只能说是经验不够，自己也不是很懂轮足的控制，仿真上的方向基本上是试出来的。
2. 对于一些细节问题做的不够好，这次差点没调出来却也是因为一个小问题（轮子半径给错了），还好及时发现问题，不然就要被老师push惨了。
3. 轮足的推导一定要自己去认真的推导一遍，不要靠着现成的公式和模型来做，一定要自己做，一定要确定好正方向的问题，不然就会导致致命的错误甚至机构损坏。第一代轮腿没调好主要原因就在于这里，现在想想，当时的情况就是轮子的转动方向给反了，所以导致没站起来，这一点还是挺傻鸟的一个问题，尚且归于经验不足吧。
4. 对于实物的转动惯量不好测出，可以利用solidworks，之前一代轮腿只是粗略的量了质量等，然后在solidwork上的模型也与实际的模型相差甚远，所以最终得出的效果不会很好。所有一个本办法，就是把每一个零件的质量都测出来，假设零件内部的质量分布均匀，那这个模型会是非常准确的，最终得到的转动惯量会很好，效果也就很不错。

3.17

之前调试中一直出现的抖动的问题难以解决，但是早上一次偶然的一次发现，腿部连杆转动惯量的拟合出来的参数的置信度非常小，只有0.02，几乎是不可信的，但是腿部连杆最终也是可以有一个平衡的表现，但就是会有来回抖动，并且模型不稳定，时不时会有高频小抖动，而且起身站立那一刻也是会有大幅度的抖动。当我把腿部连杆质量减少到原来的一半的时候，系统依旧可以稳定，而且状态更好，但是会偶尔有连续高频抖动导致系统不稳定甚至失衡。当然这个问题是想要说明设计机构时腿部连杆的重量一定要轻，转动惯量小（当然是相对于整个车体来说，当腿部质量只占机体质量的 0.1 时，拟合出的 K 值的系数的置信度就已经能达到 95% 以上了）就越好，而轮子的质量与转动惯量对最终的拟合结果几乎没有影响

所以机械组在设计机构时要着重注意这一点，这将会导致最终的效果

5.5

更新了腿部 PID 参数，腿部 PID 参数一定不要太大，PID 输出太大导致权重系数不能调大，这很致命，导致稳定性下降，一定要把 PID 参数调的尽量小，还差个 roll 角补偿，这个还好说，可以使用运动学分析，来调整腿长关系来实现

好久没有更新这代码了，因为用到了另一块板子，程序不是在 keil 上跑的，所以直接移植到哪个地方去了，这个代码实际上还不够完善，之后有时间可以再完善/重构一下代码