电机部分的控制是机器人底盘实现中，最为主要的部分。对于需要自主定位导航的服务机器人而言，电机里程计的精准度，往往是决定整个机器人定位精度的关键因素之一。因此，
本文将概述常见机器人底盘的电机及其编码器并结合Breakout 6.0中STM32的参考代码，对SLAMWARE系统中用到的里程计进行说明。 |
代码链接：slamwarekit_reference_v6_code.20181120.zip
常用的机器人底盘电机编码器按实现原理来分类，包括光电编码器及霍尔编码器，按照其编码方式分类，主要包括增量型和绝对型。对于基于slamware的机器人底盘来说，里程计的分辨率需要在1mm以下，且总误差最多不能超过5%，如果超过此数值，机器人将无法正常实现定位导航的功能。因此，无论选择哪种编码器，必须要达到其精度的要求。可以参考以下判断公式：
（2π/每转编码器脉冲数）×轮子半径≤0.001米 注：轮子半径单位为米 |
仅以两轮差动电机为例，三轮全向轮电机请参考KBSW180148 Instruction for Integrating SLAMWARE Solution in Tri-omini-wheeled Base |
SLAMWARE Core 每间隔delta时间，会向底盘发送外部系统运动速度并获取上一个周期的Deadreckon 数据。外部系统运动速度包含x轴向速度、y轴向速度和角速度。对应的，底盘MCU需要应答回复的Deadreckon数据包含x轴向位移、y轴向位移和角度位移，即(SET_V_AND_GET_DEADRECKON(0x41)。
SLAMWARE Core发送SET_V_AND_GET_DEADRECKON的请求数据包为：
X 轴向速度量：s32类型， X 轴向速度(m/s)，Q16 定点小数。
Y 轴向速度量：s32类型， Y 轴向速度(m/s)，Q16 定点小数。
角速度量：s32类型， 逆时针角速度(rad/s)，Q16 定点小数。
对应报文结构定义如下：
typedef struct _base_set_velocity_request
{
_s32 velocity_x_q16;
_s32 velocity_y_q16;
_s32 angular_velocity_q16;
} __attribute__((packed)) base_set_velocity_request_t; |
底盘通过响应SET_V_AND_GET_DEADRECKON, 将上一周期的里程计数据发给SLAMWARE Core，应答数据包为：
X 轴向位移：s32类型，外部系统整体相对于上一次应答此请求时的 X 轴向位移(mm)，Q16 定点小数。
Y 轴向位移：s32类型，外部系统整体相对于上一次应答此请求时的 Y 轴向位移(mm)，Q16 定点小数。
角度位移：s32类型， 外部系统整体相对于上一次应答此请求时的逆时针角度位移(度)，Q16 定点小数。
对应报文结构定义如下：
typedef struct _base_deadreckon_response
{
_s32 base_dx_mm_q16;
_s32 base_dy_mm_q16;
_s32 base_dtheta_degree_q16;
_u8 status_bitmap;
} __attribute__((packed)) base_deadreckon_response_t; |
case SLAMWARECORECB_CMD_SET_V_AND_GET_DEADRECKON:
{
base_deadreckon_response_t ans_pkt;
calculateMotorStatus(&ans_pkt);
base_set_velocity_request_t *req = (base_set_velocity_request_t*)request->payload;
float speed_l_mm = (float)req->velocity_x_q16 * 1000.0 / (1 << 16);
float speed_r_mm = speed_l_mm;
float line_speed_mm = (float)req->angular_velocity_q16 / (1 << 16) * robot_radius_mm;
speed_l_mm -= line_speed_mm;
speed_r_mm += line_speed_mm;
set_walkingmotor_speed((int32_t)bumpermonitor_clamp_motorcmd(speed_l_mm), (int32_t)bumpermonitor_clamp_motorcmd(speed_r_mm));
if(speed_l_mm != 0)
{
last_motor_l_speed = (int)speed_l_mm;
}
if(speed_r_mm != 0)
{
last_motor_r_speed = (int)speed_r_mm;
}
ans_pkt.status_bitmap |= (is_ontheground()?0:BASE_MOTOR_TRACTION_LOST); // 该字段用以报告电机异常状态，用户可根据自身需求参考代码添加判断比如判断电机是否离地等，如无异常请务必都填0即ans_pkt.status_bitmap = 0;
net_send_ans(channel, &ans_pkt, sizeof(base_deadreckon_response_t));
}
break; |
以两轮差动机器人为列，按照上述代码，当机器人收到该指令时，应当：
1.根据左右轮的里程累积计算三个位移量（dx, dy, dyaw）。
2.将指令中提供的速度量（vx, vy, omega）转换成左右轮的线速度（vl, vy）。
3.返回第一步中的计算结果。
1） 变量定义：
2） 计算公式：
dyaw=(dr-dl)/2R
dx=((dl+dr)/2)*cos(dyaw)
dy=((dl+dr)/2)*sin(dyaw)
注：R 是机器人轮距半径，单位为 m，下文同。
3） 代码示例
float d_yaw = (d_dist_r_mm_f - d_dist_l_mm_f)/2.0f/robot_radius_mm; float displacement = (d_dist_l_mm_f + d_dist_r_mm_f)/2.0f; float dx = cos(d_yaw)*displacement; float dy = sin(d_yaw)*displacement; ans_pkt->base_dx_mm_q16 = (_32)(dx*(1<<16)); ans_pkt->base_dy_mm_q16 = (_32)(dy*(1<<16)); ans_pkt->base_dtheta_degree_q16 = (_32)(d_yaw/M_PIF*180*(1<<16)) |
1） 变量定义
2） 计算公式(以二轮差动模型为例)
vl = vx − omega ∗ R
vr = vx + omega ∗ R
3） 代码示例
base_set_velocity_request_t *req = (base_set_velocity_request_t*)request→payload; float speed_l_mm = (float)req->velocity_x_q16 * 1000.0 / (1 << 16); float speed_r_mm = speed_l_mm; float line_speed_mm = (float)req->angular_velocity_q16 / (1 << 16) * robot_radius_mm; speed_l_mm -= line_speed_mm; speed_r_mm += line_speed_mm; |
每米编码器脉冲数ODOMETER_EST_PULSE_PER_METER，需要根据每转编码器脉冲数以及轮子的直径来确定，公式如下：
|
//每米编码器脉冲数
#define ODOMETER_EST_PULSE_PER_METER 6390UL
//行走电机速度控制频率：60hz
#define CONF_MOTOR_HEARTBEAT_FREQ 60
#define CONF_MOTOR_HEARTBEAT_DURATION (1000/(CONF_MOTOR_HEARTBEAT_FREQ))
/*
* 刷新行走电机的里程数据函数
*/
static void _refresh_walkingmotor_odometer(_u32 durationMs)
{
_u8 cnt;
float dist_mm; // disable interrupt
_u32 irqSave = enter_critical_section(); //临界资源保护
for (cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt) //得到这段时间内的编码器数据
{
_lastEncoderTicksDelta[cnt] = _encoderTicksDelta[cnt];
_encoderTicksDelta[cnt] = 0;
}
leave_critical_section(irqSave);
if (durationMs == 0) //防止除零
durationMs = 1;
for (cnt = 0; cnt < WALKINGMOTOR_CNT; ++cnt) //根据这段时间内的编码器数据计算这段时间内速度，即当前速度
{
dist_mm = (float)_lastEncoderTicksDelta[cnt] * (1000.0/ODOMETER_EST_PULSE_PER_METER);
_lastOdometerSpeedAbs[cnt] = dist_mm * 1000.0 / durationMs;
dist_mm += _motorDistTailing[cnt];
_motorDistAccumulated[cnt] += (_u32)dist_mm;
_motorDistTailing[cnt] = dist_mm - (_u32)dist_mm;
_motorDeltaTicks[cnt] += _lastEncoderTicksDelta[cnt];
}
}
/*
* 计算左行走电机里程数据
*/
float walkingmotor_delta_ldist_mm_f(void)
{
_u32 delta_dist = _motorDeltaTicks[WALKINGMOTOR_LEFT_ID]; //计算左行走电机的累计编码器值
_motorDeltaTicks[WALKINGMOTOR_LEFT_ID] = 0;
return delta_dist * (1000.f / ODOMETER_EST_PULSE_PER_METER);
}
/*
* 计算右行走电机里程数据
*/
float walkingmotor_delta_rdist_mm_f(void)
{
_u32 delta_dist = _motorDeltaTicks[WALKINGMOTOR_RIGHT_ID]; ////计算右行走电机的累计编码器值
_motorDeltaTicks[WALKINGMOTOR_RIGHT_ID] = 0;
return delta_dist * (1000.f / ODOMETER_EST_PULSE_PER_METER);
}
#ifdef FEATURE_SET_V
static const float robot_radius_mm = 118.f;
static bool is_first_motor_data = true;
static int last_motor_l_speed = 0;
static int last_motor_r_speed = 0;
/*
*通过里程累积计算位移量
*/
static void calculateMotorStatus(base_deadreckon_response_t* ans_pkt)
{
memset(ans_pkt, 0, sizeof(*ans_pkt));
float d_dist_l_mm_f = walkingmotor_delta_ldist_mm_f(); //计算左行走电机累计里程
float d_dist_r_mm_f = walkingmotor_delta_rdist_mm_f(); //计算右行走电机累计里程
if(is_first_motor_data)
{
is_first_motor_data = false;
ans_pkt->base_dx_mm_q16 = 0;
ans_pkt->base_dy_mm_q16 = 0;
ans_pkt->base_dtheta_degree_q16 = 0;
}
else
{
if(last_motor_l_speed < 0)
{
d_dist_l_mm_f = -d_dist_l_mm_f;
}
if(last_motor_r_speed < 0)
{
d_dist_r_mm_f = -d_dist_r_mm_f;
}
float d_yaw = (d_dist_r_mm_f - d_dist_l_mm_f) / 2.0f / robot_radius_mm;
float displacement = (d_dist_l_mm_f + d_dist_r_mm_f) / 2.0f;
float dx = cos(d_yaw)*displacement;
float dy = sin(d_yaw)*displacement;
ans_pkt->base_dx_mm_q16 = (_s32)(dx * (1<<16));
ans_pkt->base_dy_mm_q16 = (_s32)(dy * (1<<16));
ans_pkt->base_dtheta_degree_q16 = (_s32)(d_yaw / M_PIf * 180 * (1<<16));
}
}
#endif
|