對于需要自主定位導航的服務型機器人而言，電機里程計的精準度影響著整個機器人的定位精度，電機部分的控制是實現(xiàn)機器人底盤中最為主要的部分。為了便于操作，本文將為大家介紹常見機器人底盤的電機及其編碼器并結合Breakout 3.0中STM32的參考代碼，對SLAMWARE系統(tǒng)中用到的里程計進行詳細說明。
電機編碼器類型選擇
常用的機器人底盤電機編碼器按實現(xiàn)原理來分類，包括光電編碼器及霍爾編碼器，按照其編碼方式分類，主要包括增量型和絕對型。對于基于slamware的機器人底盤來說，里程計的分辨率需要在1mm以下，且總誤差最多不能超過5%，如果超過此數(shù)值，機器人將無法正常實現(xiàn)定位導航的功能。因此，無論選擇哪種編碼器，必須要達到其精度的要求。可以參考以下判斷公式：
（2π/每轉編碼器脈沖數(shù)）×輪子半徑≤0.001米
注：輪子半徑單位為米
系統(tǒng)電機應答流程（僅以兩輪差動電機為例）
SLAMWARE Core 每間隔delta時間，會向底盤發(fā)送左右輪的速度，向前為正，向后為負，即SET_BASE_MOTOR(0x40)。底盤會回復此時的左右輪里程計的累計值，即GET_BASE_MOTOR_DATA(0x31)。請注意，無論輪子向前運動或向后運動，里程計的度數(shù)均遞增，因為SLAMWARE Core在下發(fā)速度時，已經區(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;
對應下面的代碼：
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, 將左右輪的累計里程發(fā)給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;
里程計部分代碼示例：
每米編碼器脈沖數(shù)ODOMETER_EST_PULSE_PER_METER，需要根據(jù)每轉編碼器脈沖數(shù)以及輪子的直徑來確定，公式如下：
每米編碼器脈沖數(shù)=每轉編碼器脈沖數(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時間內編碼器的脈沖數(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ù)計算這段時間內速度，即當前速度
_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%以內。
|
