关于电机里程计，你还有这些不知道的事儿

說到機器人自主定位導航解決方案及技術，大家腦海里肯定會浮現“定位”、“建圖”、“避障”、“路徑規劃”等字眼。

在思嵐提供的自主定位導航解決方案中，主要是通過雷達和里程計的數據來進行定位。雖然激光雷達在一定范圍之內會糾正里程計的誤差，但在某些特殊環境下（如雷達掃描的有效點較少時），里程計的準備度還是會影響定位的效果。

因此，對于需要自主定位導航的服務機器人而言，電機里程計的精準度，往往是決定整個機器人定位精度的關鍵因素之一。

SDP Mini 里程計

里程計類型的選擇：

常用的機器人底盤電機編碼器按實現原理來分類，包括光電編碼器及霍爾編碼器；按照其編碼方式分類，主要包括增量型和絕對型。對于基于slamware的機器人底盤來說，里程計的分辨率需要在1mm以下，且總誤差最多不能超過5%，如果超過此數值，機器人將無法正常實現定位導航的功能。因此，無論選擇哪種編碼器，必須要達到其精度的要求。可以參考以下判斷公式：

（2π/每轉編碼器脈沖數）×輪子半徑≤0.001米

注：輪子半徑單位為米

系統電機應答流程：

(以兩輪差動電機為例）

SLAMWARE Core 每間隔delta時間，會向底盤發送左右輪的速度，向前為正，向后為負，即SET_BASE_MOTOR(0x40)。底盤會回復此時的左右輪里程計的累計值，即GET_BASE_MOTOR_DATA(0x31)。

請注意，無論輪子向前運動或向后運動，里程計的度數均遞增，因為SLAMWARE Core在下發速度時，已經區分了向前還是向后。

SLAMWARE Core發送SET_BASE_MOTOR的請求報文為, 速度的單位為mm/s：

里程計代碼示例詳見：

SET_BASE_MOTOR

typedefstruct_base_set_motor_request

{

_s32 motor_speed_mm[4];

} __attribute__((packed)) base_set_motor_request_t;

對應下面的代碼：

SET_BASE_MOTOR

caseSLAMWARECORECB_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

typedefstruct_base_motor_status_response

{

_s32 motor_cumulate_dist_mm_q16[4];

} __attribute__((packed)) base_motor_status_response_t;

對應的代碼：

GET_BASE_MOTOR_DATA

caseSLAMWARECORECB_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))

/*

* 刷新行走電機的里程數據函數

*/

staticvoid_refresh_walkingmotor_odometer(_u32 durationMs)

{

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

for(size_tcnt = 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_tcnt = 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;

}

那么，對于里程計而言，我們該如何判斷其定位數據是否準確呢？下面，就要借助可擴展的機器人管理與開發應用軟件：Robo Studio

首先，先將準備工作做好：

在調試之前，請找到一處合適測試的區域，該區域需有清晰的邊界，比如，矩形的空房間。安裝好Robo Studio，并連接上機器人（如SDP Mini）。Robo Studio下載鏈接及在線文檔可至思嵐官網下載安裝。

利用Robo Studio調試里程計準確度測試方法：

1.將機器人控制到離一面直墻若干米的位置，面朝直墻，如下圖所示。

2.將之前建好的地圖清除掉，點擊清空地圖，此時，機器人當前的區域會重建。當前區域重建好之后，可以點擊暫停建圖，停止地圖更新。最后，可以將暫停定位開啟，開啟后，將關閉激光定位，僅用里程計的數據來定位。

3.控制機器人向前朝墻運動，觀察激光點是否和墻重合，如果和墻重合，證明里程計較為準確；

如果激光點超出墻，證明底盤上報的里程計數據要大于實際運動的距離，里程計偏大。

如果激光點在墻之內，證明底盤上報的里程計數據要小于實際運動的距離，里程計偏小。

如果激光點和墻重合，可以繼續控制機器人倒退，左右轉動，看看激光點是否和周圍環境匹配；