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

说到机器人自主定位导航解决方案及技术，大家脑海里肯定会浮现“定位”、“建图”、“避障”、“路径规划”等字眼。

在思岚提供的自主定位导航解决方案中，主要是通过雷达和里程计的数据来进行定位。虽然激光雷达在一定范围之内会纠正里程计的误差，但在某些特殊环境下（如雷达扫描的有效点较少时），里程计的准备度还是会影响定位的效果。

因此，对于需要自主定位导航的服务机器人而言，电机里程计的精准度，往往是决定整个机器人定位精度的关键因素之一。

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.控制机器人向前朝墙运动，观察激光点是否和墙重合，如果和墙重合，证明里程计较为准确；

如果激光点超出墙，证明底盘上报的里程计数据要大于实际运动的距离，里程计偏大。

如果激光点在墙之内，证明底盘上报的里程计数据要小于实际运动的距离，里程计偏小。

如果激光点和墙重合，可以继续控制机器人倒退，左右转动，看看激光点是否和周围环境匹配；