Page tree

Versions Compared

Key

  • This line was added.
  • This line was removed.
  • Formatting was changed.


...

 

本页内容

Table of Contents
exclude本页内容
stylecircle

...

Excerpt

本文将概述常见机器人底盘的电机及其编码器并结合Breakout 36.0中STM32的参考代码,对SLAMWARE系统中用到的里程计进行说明。

代码链接:http://www.slamtec.com/download/slamware/code/slamwarekit_reference_v6_code.2016102520181120.zip

电机编码器类型选择

常用的机器人底盘电机编码器按实现原理来分类,包括光电编码器及霍尔编码器,按照其编码方式分类,主要包括增量型和绝对型。对于基于slamware的机器人底盘来说,里程计的分辨率需要在1mm以下,且总误差最多不能超过5%,如果超过此数值,机器人将无法正常实现定位导航的功能。因此,无论选择哪种编码器,必须要达到其精度的要求。可以参考以下判断公式:

...

Info
title注意

仅以两轮差动电机为例,三轮全向轮电机请参考KBSW180145 基于Slamware解决方案的服务型机器人底盘电机里程计概述及代码示例

...

KBSW180148 Instruction for Integrating SLAMWARE Solution in Tri-omini-wheeled Base

Image Added

SLAMWARE Core 每间隔delta时间,会向底盘发送左右轮的速度,向前为正,向后为负,即SET_BASE_MOTOR(0x40)。底盘会回复此时的左右轮里程计的累计值,即GET_BASE_MOTOR_DATA(0x31)。请注意,无论轮子向前运动或向后运动,里程计的度数均递增,因为SLAMWARE Core在下发速度时,已经区分了向前还是向后。

SLAMWARE Core发送SET_BASE_MOTOR的请求报文为, 速度的单位为mm/s:

每间隔delta时间,会向底盘发送外部系统运动速度并获取上一个周期的Deadreckon 数据。外部系统运动速度包含x轴向速度、y轴向速度和角速度。对应的,底盘MCU需要应答回复的Deadreckon数据包含x轴向位移、y轴向位移和角度位移,即(SET_V_AND_GET_DEADRECKON(0x41)。

请求数据包

SLAMWARE Core发送SET_V_AND_GET_DEADRECKON的请求数据包为:

Image Added

X 轴向速度量:s32类型, X 轴向速度(m/s),Q16 定点小数。

Y 轴向速度量:s32类型, Y 轴向速度(m/s),Q16 定点小数。

角速度量:s32类型, 逆时针角速度(rad/s),Q16 定点小数。


对应报文结构定义如下:

Code Block
languagecpp
titleSET_BASE_MOTOR0x41请求数据包
typedef struct _base_set_motorvelocity_request
{    
    _s32 motorvelocity_speed_mm[4];
x_q16;
    _s32 velocity_y_q16;
    _s32 angular_velocity_q16;
} __attribute__((packed)) base_set_motorvelocity_request_t;

对应下面的代码:

应答数据包

底盘通过响应SET_V_AND_GET_DEADRECKON, 将上一周期的里程计数据发给SLAMWARE Core,应答数据包为:

Image Added

X 轴向位移:s32类型,外部系统整体相对于上一次应答此请求时的 X 轴向位移(mm),Q16 定点小数。

Y 轴向位移:s32类型,外部系统整体相对于上一次应答此请求时的 Y 轴向位移(mm),Q16 定点小数。

角度位移:s32类型, 外部系统整体相对于上一次应答此请求时的逆时针角度位移(度),Q16 定点小数。


对应报文结构定义如下:

case SLAMWARECORECB_CMD_SET_BASE_MOTOR: {
Code Block
languagecpp
titleSET_BASE_MOTOR
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;


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

示例响应代码

Code Block
languagecpp
titleSET_V_AND_GET_DEADRECKON
case SLAMWARECORECB_CMD_SET_V_AND_GET_DEADRECKON:
    {
    	base_deadreckon_response_t ans_pkt;
    	calculateMotorStatus(&ans_pkt);

      	base_set_motorvelocity_request_t *ans_pktreq = (base_set_motorvelocity_request_t *) request->payload;
        float speed_l_mm = (float)req->velocity_x_q16 * 1000.0 / (1 << if (!bumpermonitor_filter_motorcmd(ans_pkt->motor_speed_mm[0], ans_pkt->motor_speed_mm[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(ans_pkt->motor_speed_mm[0], ans_pkt->motor_speed_mm[1]);
            }
            net(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, NULL&ans_pkt, 0);
        }
    break;

底盘会通过响应GET_BASE_MOTOR_DATA, 将左右轮的累计里程发给SLAMWARE Core,响应报文为,距离单位为mm:

sizeof(base_deadreckon_response_t));
     }
     break;

处理应答逻辑

以两轮差动机器人为列,按照上述代码,当机器人收到该指令时,应当:

1.根据左右轮的里程累积计算三个位移量(dx, dy, dyaw)。

2.将指令中提供的速度量(vx, vy, omega)转换成左右轮的线速度(vl, vy)。

3.返回第一步中的计算结果。

通过里程累积计算三个位移量

1) 变量定义: 

Image Added

2) 计算公式: 

dyaw=(dr-dl)/2R

dx=((dl+dr)/2)*cos(dyaw)

dy=((dl+dr)/2)*sin(dyaw)

注:R 是机器人轮距半径,单位为 m,下文同。

3) 代码示例

Code Block
languagecpp
titleGET_BASE_MOTOR_DATA
typedef struct _base_motor_status_response
{
    _s32 motor_cumulate_dist_mm_q16[4];
    
} __attribute__((packed)) base_motor_status_response_t;

对应的代码:

Code Block
languagecpp
titleGET_BASE_MOTOR_DATA
    case SLAMWARECORECB_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;

...

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) 变量定义

Image Added

2) 计算公式(以二轮差动模型为例)

vl = vx − omega ∗ R

vr = vx + omega ∗ R

3) 代码示例

Code Block
languagecpp
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;

里程计部分代码示例

Info

每米编码器脉冲数ODOMETER_EST_PULSE_PER_METER,需要根据每转编码器脉冲数以及轮子的直径来确定,公式如下:

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


注:轮子直径单位为米
Code Block
languagecpp
titleOdometry
//每米编码器脉冲数
#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 (size_t 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 _lastEncoderTicksDelta- (_u32)dist_mm;
        _motorDeltaTicks[cnt] += _encoderTicksDeltalastEncoderTicksDelta[cnt];
    }
}

/*
 * 计算左行走电机里程数据
 */
float walkingmotor_delta_ldist_mm_f(void)
{
    _u32 delta_dist = _motorDeltaTicks[WALKINGMOTOR_LEFT_ID];  //获得delta时间内编码器的脉冲数计算左行走电机的累计编码器值
           _motorAccumulatedTicks[cnt] += _encoderTicksDelta[cnt];                 //获得累计编码器的脉冲数
        _encoderTicksDelta[cnt_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 /    leave_critical_section(irqSaveODOMETER_EST_PULSE_PER_METER);
}



if (durationMs == 0)                                                        //防止除零
        durationMs = 1;
    for (size_t cnt = 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;
}

里程计测试

可以用slamware_console工具中的run以及vrun命令来测试里程计的准确性,其误差需要在5%以内。

slamware_console工具:http://www.slamtec.com/download/slamware/tools/slamware_console-20161012.zip

slamware_console工具使用指南:KBSW180145 基于Slamware解决方案的服务型机器人底盘电机里程计概述及代码示例

#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