亚洲色婷婷综合久久| 精品国产一区二区三区香蕉欧美|久久在精品线影院精品国产|国产中日韩久久久噜噜久久|
主页
产品
应用案例
新闻动态
购买渠道
下载与支持
关于我们
生态合作
联系我们
主页
产品
新闻动态
购买渠道
下载与支持
关于我们
加入我们
联系我们
  • 中文|
  • Eng
  • 基于SLAMWARE的服务型机器人底盘电机里程计说明及代码示例

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

    代碼鏈接:http://www.shhzfl.com/download/slamware/code/slamwarekit_reference_code.20161025.zip

    電機編碼器類型選擇

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

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

    注:輪子半徑單位為米

     

    系統(tǒng)電機應(yīng)答流程(僅以兩輪差動電機為例)

    服務(wù)機器人底盤電機

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

    SLAMWARE Core發(fā)送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;

    對應(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;

    底盤會通過響應(yīng)GET_BASE_MOTOR_DATA, 將左右輪的累計里程發(fā)給SLAMWARE Core,響應(yīng)報文為,距離單位為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;

    對應(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;

     

    里程計部分代碼示例:

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

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

    注:輪子直徑單位為米

     

    Odometry

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

    #define ODOMETER_EST_PULSE_PER_METER  6390UL

     

    //行走電機速度控制頻率:60hz

    #define CONF_MOTOR_HEARTBEAT_FREQ     60

    #define CONF_MOTOR_HEARTBEAT_DURATION (1000/(CONF_MOTOR_HEARTBEAT_FREQ))

     

    /*

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

     */

    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時間內(nèi)編碼器的脈沖數(shù)

            _motorAccumulatedTicks[cnt] += _encoderTicksDelta[cnt];                 //獲得累計編碼器的脈沖數(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ù)計算這段時間內(nèi)速度,即當前速度

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

        }

    }

    /*

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

     * 單位:mm

     */

    _u32 cumulate_walkingmotor_ldist_mm(void)

    {

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

    }

    /*

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

     * 單位:mm

     */

    _u32 cumulate_walkingmotor_rdist_mm(void)

    {

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

    }

    里程計測試

    可以用slamware_console工具中的run以及vrun命令來測試里程計的準確性,其誤差需要在5%以內(nèi)。

     

    slamware_console工具:

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

     

    關(guān)鍵字:激光雷达,SLAM

    top
    亚洲色婷婷综合久久| 精品国产一区二区三区香蕉欧美|久久在精品线影院精品国产|国产中日韩久久久噜噜久久|