Page tree
Skip to end of metadata
Go to start of metadata

You are viewing an old version of this page. View the current version.

Compare with Current View Page History

« Previous Version 12 Next »

 


本页内容


概述

电机部分的控制是机器人底盘实现中,最为主要的部分。对于需要自主定位导航的服务机器人而言,电机里程计的精准度,往往是决定整个机器人定位精度的关键因素之一。因此,本文将概述常见机器人底盘的电机及其编码器并结合Breakout 3.0中STM32的参考代码,对SLAMWARE系统中用到的里程计进行说明。代码链接:http://www.slamtec.com/download/slamware/code/slamwarekit_reference_code.20161025.zip

电机编码器类型选择

常用的机器人底盘电机编码器按实现原理来分类,包括光电编码器及霍尔编码器,按照其编码方式分类,主要包括增量型和绝对型。对于基于slamware的机器人底盘来说,里程计的分辨率需要在1mm以下,且总误差最多不能超过5%,如果超过此数值,机器人将无法正常实现定位导航的功能。因此,无论选择哪种编码器,必须要达到其精度的要求。可以参考以下判断公式:

(2π/每转编码器脉冲数)×轮子半径≤0.001米


 注:轮子半径单位为米

系统电机应答流程

注意

SLAMWARE Core 每间隔delta时间,会向底盘发送左右轮的速度,向前为正,向后为负,即SET_BASE_MOTOR(0x40)。底盘会回复此时的左右轮里程计的累计值,即GET_BASE_MOTOR_DATA(0x31)。请注意,无论轮子向前运动或向后运动,里程计的度数均递增,因为SLAMWARE Core在下发速度时,已经区分了向前还是向后。

SLAMWARE Core发送SET_BASE_MOTOR的请求报文为, 速度的单位为mm/s:

SET_BASE_MOTOR
typedef struct _base_set_motor_request
{
    _s32 motor_speed_mm[4];
} __attribute__((packed)) base_set_motor_request_t;

对应下面的代码:

SET_BASE_MOTOR
    case SLAMWARECORECB_CMD_SET_BASE_MOTOR:
        {
            base_set_motor_request_t *ans_pkt = (base_set_motor_request_t *) request->payload;
            if (!bumpermonitor_filter_motorcmd(ans_pkt->motor_speed_mm[0], ans_pkt->motor_speed_mm[1])) {
                set_walkingmotor_speed(ans_pkt->motor_speed_mm[0], ans_pkt->motor_speed_mm[1]);
            }
            net_send_ans(channel, NULL, 0);
        }
    break;

底盘会通过响应GET_BASE_MOTOR_DATA, 将左右轮的累计里程发给SLAMWARE Core,响应报文为,距离单位为mm:

GET_BASE_MOTOR_DATA
typedef struct _base_motor_status_response
{
    _s32 motor_cumulate_dist_mm_q16[4];
    
} __attribute__((packed)) base_motor_status_response_t;

对应的代码:

GET_BASE_MOTOR_DATA
    case SLAMWARECORECB_CMD_GET_BASE_MOTOR_DATA:
        {
            base_motor_status_response_t ans_pkt;
            memset(&ans_pkt, 0, sizeof(ans_pkt));          
            ans_pkt.motor_cumulate_dist_mm_q16[0] = (_s32) (cumulate_walkingmotor_ldist_mm());
            ans_pkt.motor_cumulate_dist_mm_q16[1] = (_s32) (cumulate_walkingmotor_rdist_mm());
            net_send_ans(channel, &ans_pkt, sizeof(base_motor_status_response_t));
        }
        break;


里程计部分代码示例:

每米编码器脉冲数ODOMETER_EST_PULSE_PER_METER,需要根据每转编码器脉冲数以及轮子的直径来确定,公式如下:

每米编码器脉冲数=每转编码器脉冲数/(π×轮子直径)


注:轮子直径单位为米


Odometry
//每米编码器脉冲数
#define ODOMETER_EST_PULSE_PER_METER  6390UL


//行走电机速度控制频率:60hz
#define CONF_MOTOR_HEARTBEAT_FREQ     60
#define CONF_MOTOR_HEARTBEAT_DURATION (1000/(CONF_MOTOR_HEARTBEAT_FREQ))


/*
 * 刷新行走电机的里程数据函数
 */
static void _refresh_walkingmotor_odometer(_u32 durationMs)
{
    _u32 irqSave = enter_critical_section();                                    //临界资源保护
    for (size_t cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt) {                       
        _lastEncoderTicksDelta[cnt] = _encoderTicksDelta[cnt];                  //获得delta时间内编码器的脉冲数
        _motorAccumulatedTicks[cnt] += _encoderTicksDelta[cnt];                 //获得累计编码器的脉冲数
        _encoderTicksDelta[cnt] = 0;
    }
    leave_critical_section(irqSave);
    if (durationMs == 0)                                                        //防止除零
        durationMs = 1;
    for (size_t cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt) {                       //根据delta的编码器数据计算这段时间内速度,即当前速度
        _lastOdometerSpeedAbs[cnt] = (float) _lastEncoderTicksDelta[cnt] * (1000.0 / ODOMETER_EST_PULSE_PER_METER) * 1000.0 / durationMs;
    }
}
/*
 * 计算左行走电机累计里程函数
 * 单位:mm
 */
_u32 cumulate_walkingmotor_ldist_mm(void)
{
    return (_motorAccumulatedTicks[WALKINGMOTOR_LEFT_ID] * 1000) / ODOMETER_EST_PULSE_PER_METER;
}
/*
 * 计算右行走电机累计里程函数
 * 单位:mm
 */
_u32 cumulate_walkingmotor_rdist_mm(void)
{
    return (_motorAccumulatedTicks[WALKINGMOTOR_RIGHT_ID] * 1000) / ODOMETER_EST_PULSE_PER_METER;
}

里程计测试

可以用slamware_console工具中的run以及vrun命令来测试里程计的准确性,其误差需要在5%以内。

slamware_console工具:http://www.slamtec.com/download/slamware/tools/slamware_console-20161012.zip

slamware_console工具使用指南:SA003 基于Slamware解决方案的服务型机器人底盘电机里程计概述及代码示例



  • No labels