DIY桌面机械臂__FOC电机驱动

### DIY桌面机械臂__FOC电机驱动器（#002）

---

前言

​ 之前写了个设计桌面机械臂的文稿，本来按照流程要先对做一些基础的仿真和测试、可行性分析，但是后来觉得可以在实作过程中边研究边制作；而且看了一下高级的伺服电机机器人节点咱也买不起，就想从零开始学习并设计机器人模组，本人不是机械工程出生，但对控制类的系统有浓厚兴趣，便开始执行并在博客中分享该项目的过程，并希望能和大家一起讨论。

一、硬件框架与模型

​ 设计机械臂最核心的部分应该就是关节部分的伺服电机了，针对与DIY文稿中的设计思路，每个伺服电机都为一独立的控制系统，并通过CAN总线的形式获取数据并控制。

​ 测试电机选择2804云台电机，是个7个极对数的无刷电机，至于为什么选这个，淘宝上几元就能买到。初步考虑，硬件驱动的pcb部分与电机安装一体。

1. 硬件原理图

​ 三相MOS驱动器用的是L6234P，最大电压52V，瞬时最大电流5A，让这个小电机转起来应该是绰绰有余。单片机还是选用比较常用的STM32F103C8T6，但是最近价格涨的厉害，就换成了GD32同类型，传感器采用磁感应器件AS5600，角度分辨率是12位，电压采用12V供电，通过MT2492 DCDC降压到5V。

​ 这里给出无刷电机驱动器的硬件原理图与PCB图：

​

​

​ 底层 顶层

2. 模型与安装

​ 为了方便设计与安装，这里把电机和驱动板绘制成3D模型，具体如下：

​ 至于为什么设计的电路板比电机大一圈，看起来不紧凑，是因为为了安装孔的位置以及较多的元器件，实在无法缩小板子的体积，但是对后面的设计与测试影响不是很大。

二、电路制作与测试

1. 电路制作与修改

​ 拿到板子和器件后，立马进行焊接与测试，下图是单面焊接完成的情况。

​ 直到编写软件才发现在原理图中根本就没有AS5600的接口，往往检查好pcb的规则后就应该是设计投板的最后一步了，这样的错误确实足够愚蠢的,直接通过跳线到PB11,PB10的两个引出来的焊盘上，才通过程序测试。

​ 关于中间那部分支撑是根据电路板的结构去设计，但是感觉这样设计的问题在于每修改一次电路后，与之接触的部分模块都要重新设计。!

2. 程序设计与pid角度控制

• 首先要做的就是把单个模块都调通，再进行下一步的操作

• 其次再进行pid的参数调节，达到比较合适的状态

  2.1 AS5600的传感器

  首先进行AS5600的传感器参数读取，并通过串口打印出来观察，一开始对于I2C协议，想到的是STM32的硬件I2Cmaster，因为相较于模拟IO，响应会快一些，并且不会占用cpu资源，但是后来测试发现系统的中断对快速读取的I2C程序部分有影响，会直接卡死，所以后来还是选择了模拟IO的方式。

2.2 SVPWM控制开环

这里采用了simple foc的设计，并实现了电机的开环运行。

2.3 闭环pid控制

​ 给出控制电机的核心代码，由于没有加积分，所以存在的静态误差让人很难以接受，后续再进行改进

void my_close_p_con(void)
{
      float a_er=0;
      float s_a=0;
      float an_p_cofe=15;    //角度的比例参数
      float vq_p_cofe=30;    //力矩的比例参数
      float vq_d_cofe=450;	 //力矩的微分参数
      static float a_er_l=0;
      float an_pid_o=0;    //角度pid输出
      float vq_pid_o=0;	   //力矩pid输出
      float an_pid_max=0.3;  //最大牵引角度(我也不知道为啥我要这么命名)
      float d_dat=0;		//微分量
	  float ang_shift=0;    //绝对的角度控制参数

      s_a=FilterDeal(getAngle());  //滤波后的电机角度值
      a_er=target_angle-s_a;   		//误差值
      
      an_pid_o=a_er*an_p_cofe;   //角度只有比例控制
     
      if(an_pid_o>an_pid_max)//限幅
      {
          an_pid_o=an_pid_max;
      }
      else if(an_pid_o<-an_pid_max)//限幅
      {
        an_pid_o=-an_pid_max;
      } 

      d_dat=a_er-a_er_l;   //微分量的输出等于这次的误差减去上一次误差
      vq_pid_o=fabs(a_er*vq_p_cofe+d_dat*vq_d_cofe);     //力矩的参数控制，比例加微分（因为震荡严重，所以加了d）
      if(vq_pid_o>max_p) //力矩限幅
      {
        vq_pid_o=max_p;
      }   
      ang_shift=init_ang_bias+s_a+an_pid_o;  //绝对角度等于静态校准偏差加上跟踪的角度值加上pid的偏移量实现角度控制
	  S_Uq=vq_pid_o;  
      move(ang_shift);   //运行

      a_er_l=a_er;  //保存误差

---

更新时间：2021 - 9 - 6

---

三、设计与测试的问题

1. 驱动器发热

​之前采用LD6234这块集成MOSFET的常用驱动器,在12V/500mA的条件下，驱动相电阻10欧姆左右的无刷电机，就已经发热严重了，这可能和pwm的工作频率和外围自举电容参数有，但是问题在于电机扭力比较小，该电机的kv大概是2000左右，不适合用来做直驱，但是很小型的低kv电机，要么就是加了减速器，成本直接就上去了，所以这里准备将这个电机模块放在机械臂的前端，后续买力矩更加合适的电机测试。

2. 扭力小&效率低

由于没有个具体的参数说明电机工作的扭力大小，这里用视频简单展示一下：

这里观察到之前说明的那个问题，就是静态误差，后来经过测试发现如何加入积分参数也解决不了，同时震荡严重，才发现是死区问题，后来在代码中加入力矩的固定偏移量尝试解决。

这是解决了死区后的效果，但静态误差依然存在。

3. 代码改善

​ 在代码中加入了死区消除与积分的参数，给出代码与最终的效果图

3.1 CODE

  void my_close_p_con(void)
  {
        float a_er=0;
        float s_a=0;
        float an_p_cofe=15;    //角度的比例参数
        float vq_p_cofe=30;    //力矩的比例参数
        float vq_d_cofe=450;	 //力矩的微分参数
        static float a_er_l=0;
        float an_pid_o=0;    //角度pid输出
        float vq_pid_o=0;	   //力矩pid输出
        float an_pid_max=0.3;  //最大牵引角度(我也不知道为啥我要这么命名)
        float d_dat=0;		//微分量
  	  	float ang_shift=0;    //绝对的角度控制参数
  
      	/*角度与力矩的积分参数*/
      	static float an_i=0;  //角度积分参数
		static float vq_i=0;  //力矩积分参数
		float uq_bias=2.6;	  //死区电压或者说是死区的力矩参数
      	float an_i_cofe=0.01; 
		float vq_i_cofe=0.1;
      	/*****************/
      	
     	#ifdef _i_used       //这里用了宏定义决定是否使用积分
		vq_i+=a_er*vq_i_cofe;
		an_i+=a_er*an_i_cofe;
		constri(&vq_i,vq_i_max,-vq_i_max);
      	constri(&an_i,an_i_max,-an_i_max);	
		#endif
      	
        s_a=FilterDeal(getAngle());  //滤波后的电机角度值
        a_er=target_angle-s_a;   		//误差值
        
        an_pid_o=a_er*an_p_cofe+an_i;  //角度只有比例控制
       	constri(&an_pid_o,an_pid_max,-an_pid_max);
  
        d_dat=a_er-a_er_l;   //微分量的输出等于这次的误差减去上一次误差
        vq_pid_o=fabs(a_er*vq_p_cofe+vq_i+d_dat*vq_d_cofe); //力矩的参数控制，比例加微分（因为震荡严重，所以加了d）
        if(vq_pid_o>max_p) //力矩限幅
        {
          vq_pid_o=max_p;
        }   
        ang_shift=init_ang_bias+s_a+an_pid_o;  //绝对角度等于静态校准偏差加上跟踪的角度值加上pid的偏移量实现角度控制
  	  	S_Uq=vq_pid_o;  
        move(ang_shift);   //运行
  
        a_er_l=a_er;  //保存误差
  }

3.2最终的控制效果

3.2 无负载下的控制精度

前者是当前被控制到的角度，单位为弧度，后者是cpu的温度，可见发热确实比较严重

四、重新设计电路板与结构

1. 栅极驱动器加MOSFET

由于发热严重，效率极低，以及本来之前就存在电路与电机的不紧凑的问题，所以重新设计了电路板，等待制作中…

电路上改成栅极驱动器和分立的MOS半桥电路，可以灵活的选择不同功率的管子，并取消了之前的电流传感器，发现并不需要做无感的驱动，给出电路图和原理图：

Sheet

Top
Button

2. 尺寸与模型

​ 设计相关结构并给出3D的安装图纸：

虽然电路还未到，也不知道能够测试成功，但是感觉想设计好还是挺难得，而且这只是开始，后面还要考虑的机械框架，以及是怎么样的结构，对于机械建模经验并不丰富的我来说还是具有挑战性的。

---

更新时间 2021 - 9 - 9

---

持续更新，一起交流！！！