本页内容
概述
电机部分的控制是机器人底盘实现中,最为主要的部分。对于需要自主定位导航的服务机器人而言,电机里程计的精准度,往往是决定整个机器人定位精度的关键因素之一。因此,本文将概述常见机器人底盘的电机及其编码器并结合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