本页内容


概述

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


对应报文结构定义如下:

0x41请求数据包
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 定点小数。


对应报文结构定义如下:

0x41应答数据包
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;


示例响应代码及处理应答逻辑

示例响应代码

SET_V_AND_GET_DEADRECKON
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,需要根据每转编码器脉冲数以及轮子的直径来确定,公式如下:

每米编码器脉冲数=每转编码器脉冲数/(π×轮子直径)


注:轮子直径单位为米
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))


/*
 * 刷新行走电机的里程数据函数
 */
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