1. FOC常用驱动硬件方案介绍

目前的电驱动四足机器人中关节伺服驱动是最关键的电子模块,其完成对无刷电机的驱动以产生持续的力矩输出从而实现机器人所需要的力矩控制,对于无刷电机的伺服驱动目前主要采用FOC矢量控制算法,基于该算法已经有许多成熟的开源方案如SimpleFOC和著名MIT开源项目提供的基于STM32的驱动电路设计,基于该方案目前淘宝上有许多电机厂商都推出了自己的一体化伺服驱动电机,如智擎、T-motor、INFOS等:

基于Benjamin Katz 的开源方案是目前较成熟的关节伺服驱动设计,相关链接如下

除了MIT本身使用该驱动器完成对自己四足机器人的搭建外,有很多极客基于该方案设计了自己的驱动单元如YouTube知名极客Josh Pieper基于该驱动方案自己加工减速组和机械完成的目前尺寸最小的高性能四足机器人:

可见无刷驱动器对于四足机器人来说是最重要的组成部分,目前国内也有许多极客开始研究相关无刷驱动单元,其中大部分基于VESC也就是本杰明电调制作了很多电动滑板,但受限于其硬件和软件不足该驱动器在低速下的力矩控制能力较差,我之前基于该方案进行了相关研究通过修改FOC控制提高了对低速力矩的控制能力,下面是基于Keil5下的开发代码可以参考(提取码:puqv):

不久前知乎稚晖君也公布设计了一款伺服驱动器zhuanlan.zhihu.com/p/14,但目前还没有具体公布开源资料,通过其CAD设计可以大概看到其采用的驱动电路和硬件设计方案与MIT和ODrive等国外知名的伺服开源驱动方案较一致,如果后续开源可以期待使用,但在等待其公布过程中我还是介绍一下目前国外许多极客在制作机器人中首选的Odrive驱动器。

ODrive是一个基于STM32的高性能FOC伺服驱动器,其可以同时驱动两个无刷电机,早期被大量使用与3D打印机或自制CNC车床电气系统的设计中,知名极客James Bruton使用其作为大部分自己制作机器人伺服驱动的核心单元,之前Stanford开源的8自由度四足机器人Doggo同样也采用了Odrive作为核心的驱动器。

Doggo基于Odirve的驱动方案

Odrive目前常用的硬件版本是3.5和3.6其在淘宝上最便宜为300~400不等,Odrive电路板上预留了USB调试接口,CAN通讯接口并支持ABI和SPI等多种编码器输入格式(当前主要还是以ABI增量式编码器为主),板载MOS管能支持25A以上的持续电流输出,外部供电支持24~48V满足大扭力电机供电需求并且有制动电阻接口来提高对系统在冲击载荷下的保护能力。综上,Odrive可以说是目前成本最低的高性能FOC驱动器首选,但在我接触使用后感觉其最大的问题是配置流程较为复杂,Odrive相关配置的详细说明可以参考CSDN上博文:

总体来说当我拿到第一块Odrive板子时花了一周最终也没有实现电机的转动,总结Odrive硬件和软件上的几个缺点如下:

(1)电机标定流程较为复杂,相关配置保存需要多次重启驱动器来验证;

(2)外部通讯控制电机的接口易用性差,目前许多项目都主要基于USB接口配合树莓派等控制器完成对电机的驱动,但如果想使用CAN或串口等更底层的硬件接口缺少相关例子,并且保护机制不明确存在危险性;

(3)电路板大部分接口均为2.54排针,接线牢固性和可靠性较差,并且接线顺序容易混乱,接口标准化程度较低,难以满足机器人需求;

2. Odrive Moco标准接口板

为解决上述问题。我基于Doggo开源资料设计了一套适用于Odrive-3.5的标准接口板套件,该套件首先对电路板硬件接口进行了整合,采用带锁扣和快速插拔式端子引出所需的通讯,调试与编码器接口;为方便目前四足机器人电气设计中常用的CAN通讯,我设计了一个接口板将Odrive串口的ASIIC协议转换为了单帧的CAN通讯协议,从而可以完成对两路电机角度和电流的回传,并且结果直接完成了减速比和电机朝向的转换,用户能自定义控制模式直接完成对电机电流、转速和位置的CAN总线命令控制,完成对典型电流限幅等参数的在线配置;同时设计了上位机,在我已有舵狗的Moco通用控制器OCU界面中扩展出了对Odrive标定专用的界面,能快速、傻瓜式地完成对电机转向、力矩系数的快速标定,并且实时显示位置闭环等波形系数方便参数调节;在底层构建了完善的保护机制,具有超速飞车保护、通讯丢失等保护,另外为方便多圈角度标零,支持一键快速标零。

(1)Odrive-Moco接口板套件

Odrive-Moco接口板套件硬件组成如下图所示,其主要包括编码器接口板:采用PH1.25带锁端子引出两路ABI编码器输入接口;编码器板,与接口板配套板载AS5047带检索高性能磁编码器;SWD下载板,引出Odrive板载STM32下载接口;通讯接口板,使用STM32F4采用串口以ASIIC协议与Odrive采用5Mbps波特率高速通讯,扩展一路1Mbps CAN完成与主控制器通讯,同时预留USB接口与OCU界面调试并支持以USB接口完成固件更新。

目前该接口套件硬件支持Odrive-3.5和3.6版本接口定义顺序,并且均支持正反面焊接的方式但可能部分引出端子需要替换为侧贴方式,相关接口板载Odrive上的安装区域如下:

其中①接口板与Odrive的GND和1,2串口收发引脚相连,其供电采用主控通过CAN接口提供5V电源实现;编码器接口板②与Odrive M1和M0电机ABI编码器接口连接;SWD接口板③与Odrive STM32下载引脚连接,同时接口板输入为5V采用降压模块至3.3V完成下载程序是对Odrive的供电;编码器板与电机连接的示意图如下:

基于该接口方案能快速将Odrive改造成机器人所需的高性能伺服驱动器,同时接线方便并保证接口可靠、简洁,我基于该套件实现的一个8自由度机器人单腿驱动模组走线效果如下:

(2)Odrive-Moco固件更新与电机标定

Odirve默认采用ABI编码器,因此在上电时均会首先自动旋转电机完成对电气角度的标定,我这里采用Doggo开源方案中提供的修改版源码并对串口通信协议二次修改完成对双电流命令下收发协议的裁剪,将其原始得应答式反馈修改为自动上传编码器角度与Iq相电流,基于Moco接口板使用Odrive最大的优势在于,上电标定一次之后由于接口板与Odrive 的通讯不会断,在我增加多重保护机制后能实现主控上下电热插拔时不会造成Odrive电路板通讯保护,因为当前保护后必然需要重新上电和电角度标定,这对代码开发和调试带来了不小的时间成本,并且也不方便使用这也是为何国外极客大部分均采用树莓派以USB口通讯控制的原因,则要使用Moco接口板首先需要对淘宝购买的原始Odrive固件进行更新,这里我提供了上电电机标定最多转动45°与90°的两个3.5硬件适配的固件,转动角度越大最终的电气标定结果越高并且越不容易在标定过程中失败,而电机上电转动顺序可以通过交换三相线实现。

A.因此首先是要Stlink链接SWD转接板使用STM32 ST-LINK Utility下载更新固件:

B. 在固件更新完成后,完成编码器、动力电、电机三相线的连接,之后连接USB连接Odrive通电,使用zadig-2.5完成对Odrive USB驱动的更新:

B. 通过在Options中List All Devices选择Odrive USB接口2,之后选择驱动为libusb-win32 (v1.2.6.0)点击安装等待安装完毕;

C. 之后再控制台或Vscode中运行如下的Python配置文件完成对电机的一键自动标定:

#!/usr/bin/env python3
"""
Example usage of the ODrive python library to monitor and control ODrive devices
"""

from __future__ import print_function

import odrive
from odrive.enums import *
import time
import math
from fibre import Logger, Event
from odrive.utils import OperationAbortedException
from fibre.protocol import ChannelBrokenException
import sys

def set_gains(odrv,axis):
    """
    Sets the nested PID gains to a good default
    """
    axis.controller.config.pos_gain = 100.0 #f [(counts/s) / counts]
    axis.controller.config.pos_gain = 0.01 #f [(counts/s) / counts]
    axis.controller.config.vel_gain = 5.0 / 10000.0 #[A/(counts/s)]
    axis.controller.config.vel_limit = 50000.0
    axis.controller.config.vel_integrator_gain = 0 #[A/((counts/s) * s)]

    axis.controller.config.kp_theta = 0.04 *  6000 / (2 * math.pi)
    axis.controller.config.kd_theta = 5.0 / 10000.0 *  6000 / (2 * math.pi)
    axis.controller.config.kp_gamma = 0.0 *  6000 / (2 * math.pi)
    axis.controller.config.kd_gamma = 5.0 / 10000.0 *  6000 / (2 * math.pi)

def calibrate_motor(odrv, axis):
    # time.sleep(0.5)
    print('Calibrating motor...',end='',flush=True)
    run_state(axis, AXIS_STATE_MOTOR_CALIBRATION, True)
    axis.motor.config.pre_calibrated = True# then set motor pre calibrated to true
    print('Done');

def calibrate_motor_and_zencoder(odrv,axis):
    """
    Runs motor and encoder calibration (with z index) and saves
    Enables closed loop control on startup and encoder search on startup
    """

    calibrate_motor(odrv, axis)

    print('Calibrating encoder...',end='',flush=True)
    axis.encoder.config.use_index = True
    run_state(axis, AXIS_STATE_ENCODER_INDEX_SEARCH, True)
    run_state(axis, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, True)
    axis.encoder.config.pre_calibrated = True
    print('Done');

    print('Setting startup flags...',end='',flush=True)
    # axis.config.startup_encoder_index_search = True
    axis.config.startup_encoder_index_search = True
    axis.config.startup_closed_loop_control = True
    print('Done with axis.')

def run_state(axis, requested_state, wait):
    """
    Sets the requested state on the given axis. If wait is True, the method
    will block until the state goes back to idle.

    Returns whether the odrive went back to idle in the given timeout period,
    which is 10s by default.
    """
    axis.requested_state = requested_state
    timeout_ctr = 100; # 20s timeout for encoder calibration to finish
    if wait:
        while(timeout_ctr > 0): # waits until state goes back to idle to continue
            time.sleep(0.2)
            timeout_ctr -= 1
            if axis.current_state == AXIS_STATE_IDLE:
                break
    return timeout_ctr > 0

def reboot_odrive(odrv):
    """
    Reboot odrive
    """
    try:
        odrv.reboot()
    except ChannelBrokenException:
        print('Lost connection because of reboot...')

def reset_odrive(odrv):
    """
    Erase config
    """
    print('Erasing config and rebooting...')
    odrv.erase_configuration()
    reboot_odrive(odrv)

def init_odrive(odrv):
    """
    NOTE: does not save
    """
    print('Initializing ODrive...')
    odrv.config.brake_resistance = 0

    odrv.axis0.motor.config.pole_pairs = 11
    odrv.axis1.motor.config.pole_pairs = 11

    odrv.axis0.motor.config.resistance_calib_max_voltage = 4.0
    odrv.axis1.motor.config.resistance_calib_max_voltage = 4.0

    odrv.axis0.encoder.config.cpr = 4000
    odrv.axis1.encoder.config.cpr = 4000

    odrv.axis0.motor.config.current_lim = 25 #40
    odrv.axis1.motor.config.current_lim = 25 #40

    odrv.axis0.motor.config.calibration_current = 10
    odrv.axis1.motor.config.calibration_current = 10

    odrv.axis0.motor.config.pre_calibrated = False
    odrv.axis1.motor.config.pre_calibrated = False

    odrv.axis0.encoder.config.pre_calibrated = False
    odrv.axis1.encoder.config.pre_calibrated = False
    print('Done.')

def set_odrive_limits(odrv,axis,cur_lim):
    """
    Set motor current limits
    """
    axis.motor.config.current_lim = current_lim

def calibrate_odrive(odrv):
    """
    Calibrate motors and encoders (including index) on both axes
    """
    print('Calibrating axes...')

    calibrate_motor_and_zencoder(odrv,odrv.axis0)
    calibrate_motor_and_zencoder(odrv,odrv.axis1)
    print('Done.')

def set_odrive_gains(odrv):
    """
    Set position control gains for both motors
    """
    print('Setting gains...')
    set_gains(odrv,odrv.axis0)
    set_gains(odrv,odrv.axis1)
    print('Done.')

def test_odrive_motors(odrv):
    """
    Give two position commands to both axis
    """
    print('Testing motors...')
    odrv.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
    odrv.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL

    time.sleep(0.5)
    odrv.axis0.controller.set_pos_setpoint(1000.0, 0.0, 0.0)
    odrv.axis1.controller.set_pos_setpoint(-1000.0, 0.0, 0.0)
    time.sleep(1)
    odrv.axis0.controller.set_pos_setpoint(0.0, 0.0, 0.0)
    odrv.axis1.controller.set_pos_setpoint(0.0, 0.0, 0.0)
    print('Done.')

def get_odrive(shutdown_token):
    """
    Look for and return an odrive connected via usb
    """

    print('Looking for ODrive...')
    odrv = odrive.find_any(search_cancellation_token=app_shutdown_token, channel_termination_token=app_shutdown_token)
    print('Found.')
    return odrv

def print_configs(odrv0):
    """
    Print configuration info
    """

    print('\n\nMOTOR CONFIG')
    print(odrv0.axis0.motor.config)
    print('\n')
    print(odrv0.axis1.motor.config)
    print('\n\n ENC CONFIG')
    print(odrv0.axis0.encoder.config)
    print('\n')
    print(odrv0.axis1.encoder.config)
    print('\n\n AXIS CONFIG')
    print(odrv0.axis0.config)
    print('\n')
    print(odrv0.axis1.config)

def full_calibration(app_shutdown_token):
    """
    Reset the odrive and calibrate everything:
    1) Gains and limits
    2) Motor
    3) Encoder index
    4) Encoder offset
    5) Set startup
    """

    # Find a connected ODrive (this will block until you connect one)
    odrv0 = get_odrive(app_shutdown_token)
    reset_odrive(odrv0)
    odrv0 = get_odrive(app_shutdown_token)

    init_odrive(odrv0)
    set_odrive_gains(odrv0)
    calibrate_odrive(odrv0)

    odrv0.save_configuration()

    #run_state(axis=odrv0.axis0, requested_state=AXIS_STATE_CLOSED_LOOP_CONTROL, wait=False);
    #run_state(axis=odrv0.axis1, requested_state=AXIS_STATE_CLOSED_LOOP_CONTROL, wait=False);
    #odrv0.axis0.controller.pos_setpoint = 0;
    #odrv0.axis1.controller.pos_setpoint = 0;

    print_configs(odrv0)

def bare_bones_calibration(app_shutdown_token, reset=True):
    """
    Just calibrate motors and basic gains
    """
    print("Run doghome\n ")# odrv0.axis0.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH   odrv0.axis0.encoder.index_found
    odrv0 = get_odrive(app_shutdown_token)
    if reset:
        reset_odrive(odrv0)
        odrv0 = get_odrive(app_shutdown_token)

    init_odrive(odrv0)
    set_odrive_gains(odrv0)

    calibrate_motor(odrv0, odrv0.axis0)
    calibrate_motor(odrv0, odrv0.axis1)

    odrv0.axis0.config.startup_encoder_offset_calibration = True
    odrv0.axis1.config.startup_encoder_offset_calibration = True
    odrv0.axis0.config.startup_closed_loop_control = True
    odrv0.axis1.config.startup_closed_loop_control = True
    
    odrv0.axis0.encoder.config.use_index = False
    odrv0.axis1.encoder.config.use_index = False
    odrv0.axis0.config.startup_encoder_index_search =False
    odrv0.axis1.config.startup_encoder_index_search =False
    odrv0.save_configuration()

    print_configs(odrv0)

def main(app_shutdown_token):
    """
    !! Main program !!
    Looks for odrive, then calibrates, then sets gains, then tests motors

    WARNING: Saving more than twice per boot will cause a reversion of all changes
    """
    bare_bones_calibration(app_shutdown_token, reset=True)
    #bare_bones_calibration(app_shutdown_token, reset=False)

import odrive
from fibre import Logger, Event
from odrive.utils import OperationAbortedException
from fibre.protocol import ChannelBrokenException

app_shutdown_token = Event()
try:
    main(app_shutdown_token)
    # init_odrive(odrv0)
except OperationAbortedException:
    logger.info("Operation aborted.")
finally:
    app_shutdown_token.set()

# encoder calibration
# set use_index ot true
# request encoder_index_search
# pre_calibrated to True
# then change startup to encoder_index_search true

代码中几个需要注意的地方如下,首先修改电机的极对数:

    odrv.axis0.motor.config.pole_pairs = 11
    odrv.axis1.motor.config.pole_pairs = 11

配置编码器的一圈脉冲数:

    odrv.axis0.encoder.config.cpr = 4000
    odrv.axis1.encoder.config.cpr = 4000

设置最大的电流:

    odrv.axis0.encoder.config.cpr = 4000
    odrv.axis1.encoder.config.cpr = 4000

设置无通讯下的Odrive内部角度控制PID参数:

    axis.controller.config.pos_gain = 100.0 #f [(counts/s) / counts]
    axis.controller.config.pos_gain = 0.01 #f [(counts/s) / counts]
    axis.controller.config.vel_gain = 5.0 / 10000.0 #[A/(counts/s)]
    axis.controller.config.vel_limit = 50000.0
    axis.controller.config.vel_integrator_gain = 0 #[A/((counts/s) * s)]
 
    axis.controller.config.kp_theta = 0.04 *  6000 / (2 * math.pi)
    axis.controller.config.kd_theta = 5.0 / 10000.0 *  6000 / (2 * math.pi)
    axis.controller.config.kp_gamma = 0.0 *  6000 / (2 * math.pi)
    axis.controller.config.kd_gamma = 5.0 / 10000.0 *  6000 / (2 * math.pi)

则运行后Odrive会首先自动擦除内部已有的配置参数,之后reboot在重启后开始对两个电机独立标定,你会听到两个电机分别哔哔声音,之后会在控制台打印相关标定参数,通过却是否有相电阻可以确认标定是否成功;

E. 在完成标定后重新上电,如标定正常电机会自动按固定方向来回旋转45或90°完成对电机参数的标定,如在标定中电机没有停下则标定正确。此时由于我们没给通讯接口板供电不会下发电流控制命令,Odrive会以之前设置的PID参数完成对标定停止后的角度进行闭环控制,此时可以用手按压电机确认闭环控制效果,如有卡顿或堵转时卸力可能是标定不正常或者Odrive驱动硬件质量不好,也就是买的板子做工不太好。

F. 之后可以使用OCU完成对电机参数的配置

(3)Odrive-Moco接口板OCU快速配置

至此已经完成了对Odrive和电机的配置,其上电后已经可以正常工作,你可以使用USB完成对电机的控制,如使用Moco接口板则需采用OCU上位机对相关参数进行设置,OCU界面如下:

上图为执行器标定界面,可以完成对两电机减速比,力矩系数和编码器等核心参数的设置,同时可以对力矩控制测量符号进行设置,也可以给定一键标零对应的角度,当然可以在该界面对位置和电流控制进行监督测试,通过波形查看当前反馈数据。本文设计的接口套件将在近期完成最终版本修改与OCU上位机配置、CAN通讯协议下一篇内容中进行更新与详细介绍

本文中相关资料下载地址如下:

链接:https://pan.baidu.com/s/1tenbee3s7GiTfNH4SoXaaw 
提取码:hkd8