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

對(duì)于需要自主定位導(dǎo)航的服務(wù)型機(jī)器人而言，電機(jī)里程計(jì)的精準(zhǔn)度影響著整個(gè)機(jī)器人的定位精度，電機(jī)部分的控制是實(shí)現(xiàn)機(jī)器人底盤(pán)中最為主要的部分。為了便于操作，本文將為大家介紹常見(jiàn)機(jī)器人底盤(pán)的電機(jī)及其編碼器并結(jié)合Breakout 3.0中STM32的參考代碼，對(duì)SLAMWARE系統(tǒng)中用到的里程計(jì)進(jìn)行詳細(xì)說(shuō)明。

代碼鏈接：http://mizpahinternationalschool.com/download/slamware/code/slamwarekit_reference_code.20161025.zip

電機(jī)編碼器類(lèi)型選擇

常用的機(jī)器人底盤(pán)電機(jī)編碼器按實(shí)現(xiàn)原理來(lái)分類(lèi)，包括光電編碼器及霍爾編碼器，按照其編碼方式分類(lèi)，主要包括增量型和絕對(duì)型。對(duì)于基于slamware的機(jī)器人底盤(pán)來(lái)說(shuō)，里程計(jì)的分辨率需要在1mm以下，且總誤差最多不能超過(guò)5%，如果超過(guò)此數(shù)值，機(jī)器人將無(wú)法正常實(shí)現(xiàn)定位導(dǎo)航的功能。因此，無(wú)論選擇哪種編碼器，必須要達(dá)到其精度的要求?？梢詤⒖家韵屡袛喙剑?/p>

（2π/每轉(zhuǎn)編碼器脈沖數(shù)）×輪子半徑≤0.001米

注：輪子半徑單位為米

系統(tǒng)電機(jī)應(yīng)答流程（僅以?xún)奢啿顒?dòng)電機(jī)為例）

SLAMWARE Core 每間隔delta時(shí)間，會(huì)向底盤(pán)發(fā)送左右輪的速度，向前為正，向后為負(fù)，即SET_BASE_MOTOR(0x40)。底盤(pán)會(huì)回復(fù)此時(shí)的左右輪里程計(jì)的累計(jì)值，即GET_BASE_MOTOR_DATA(0x31)。請(qǐng)注意，無(wú)論輪子向前運(yùn)動(dòng)或向后運(yùn)動(dòng)，里程計(jì)的度數(shù)均遞增，因?yàn)镾LAMWARE Core在下發(fā)速度時(shí)，已經(jīng)區(qū)分了向前還是向后。

SLAMWARE Core發(fā)送SET_BASE_MOTOR的請(qǐng)求報(bào)文為, 速度的單位為mm/s。

SET_BASE_MOTOR

typedef struct _base_set_motor_request

{

    _s32 motor_speed_mm[4];

} __attribute__((packed)) base_set_motor_request_t;

對(duì)應(yīng)下面的代碼：

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;

底盤(pán)會(huì)通過(guò)響應(yīng)GET_BASE_MOTOR_DATA, 將左右輪的累計(jì)里程發(fā)給SLAMWARE Core，響應(yīng)報(bào)文為，距離單位為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;

對(duì)應(yīng)的代碼：

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;

里程計(jì)部分代碼示例：

每米編碼器脈沖數(shù)ODOMETER_EST_PULSE_PER_METER，需要根據(jù)每轉(zhuǎn)編碼器脈沖數(shù)以及輪子的直徑來(lái)確定，公式如下：

每米編碼器脈沖數(shù)=每轉(zhuǎn)編碼器脈沖數(shù)/(π×輪子直徑)

注：輪子直徑單位為米

Odometry

//每米編碼器脈沖數(shù)

#define ODOMETER_EST_PULSE_PER_METER  6390UL

//行走電機(jī)速度控制頻率：60hz

#define CONF_MOTOR_HEARTBEAT_FREQ     60

#define CONF_MOTOR_HEARTBEAT_DURATION (1000/(CONF_MOTOR_HEARTBEAT_FREQ))

/*

 * 刷新行走電機(jī)的里程數(shù)據(jù)函數(shù)

 */

static void _refresh_walkingmotor_odometer(_u32 durationMs)

{

    _u32 irqSave = enter_critical_section();                                    //臨界資源保護(hù)

    for (size_t cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt) {                      

        _lastEncoderTicksDelta[cnt] = _encoderTicksDelta[cnt];                  //獲得delta時(shí)間內(nèi)編碼器的脈沖數(shù)

        _motorAccumulatedTicks[cnt] += _encoderTicksDelta[cnt];                 //獲得累計(jì)編碼器的脈沖數(shù)

        _encoderTicksDelta[cnt] = 0;

    }

    leave_critical_section(irqSave);

    if (durationMs == 0)                                                        //防止除零

        durationMs = 1;

    for (size_t cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt) {                       //根據(jù)delta的編碼器數(shù)據(jù)計(jì)算這段時(shí)間內(nèi)速度，即當(dāng)前速度

        _lastOdometerSpeedAbs[cnt] = (float) _lastEncoderTicksDelta[cnt] * (1000.0 / ODOMETER_EST_PULSE_PER_METER) * 1000.0 / durationMs;

    }

}

/*

 * 計(jì)算左行走電機(jī)累計(jì)里程函數(shù)

 * 單位：mm

 */

_u32 cumulate_walkingmotor_ldist_mm(void)

{

    return (_motorAccumulatedTicks[WALKINGMOTOR_LEFT_ID] * 1000) / ODOMETER_EST_PULSE_PER_METER;

}

/*

 * 計(jì)算右行走電機(jī)累計(jì)里程函數(shù)

 * 單位：mm

 */

_u32 cumulate_walkingmotor_rdist_mm(void)

{

    return (_motorAccumulatedTicks[WALKINGMOTOR_RIGHT_ID] * 1000) / ODOMETER_EST_PULSE_PER_METER;

}

里程計(jì)測(cè)試

可以用slamware_console工具中的run以及vrun命令來(lái)測(cè)試?yán)锍逃?jì)的準(zhǔn)確性，其誤差需要在5%以?xún)?nèi)。

slamware_console工具：

http://mizpahinternationalschool.com/download/slamware/tools/slamware_console-20161012.zip