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

Compare with Current View Page History

« Previous Version 6 Next »

 

概述

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

代码链接：http://www.slamtec.com/download/slamware/code/slamwarekit_reference_code.20161025.zip

 

电机编码器类型选择

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


 

系统电机应答流程

注意

仅以两轮差动电机为例，三轮全向轮电机请参考Slamware解决方案与三全向轮底盘集成指导

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

Slamcore发送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工具使用指南：SA001 SLAMWARE Console Application Note








  • No labels