基于SLAMWARE的服务型机器人底盘电机里程计说明及代码示例

对于需要自主定位导航的服务型机器人而言,电机里程计的精准度影响着整个机器人的定位精度,电机部分的控制是实现 机器人底盘 中最为主要的部分。为了便于操作,本文将为大家介绍常见机器人底盘的电机及其编码器并结合Breakout 3.0中STM32的参考代码,对SLAMWARE系统中用到的里程计进行详细说明。

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

电机编码器类型选择

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

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

注:轮子半径单位为米

 

系统电机应答流程 (仅以两轮差动电机为例)

基于SLAMWARE的服务型机器人底盘电机里程计说明及代码示例_第1张图片

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

 


来自 “ ITPUB博客 ” ,链接:http://blog.itpub.net/31559640/viewspace-2564739/,如需转载,请注明出处,否则将追究法律责任。

转载于:http://blog.itpub.net/31559640/viewspace-2564739/

你可能感兴趣的:(嵌入式)