【移动机器人技术】速度指令的矢量限幅处理

差动轮驱动的移动机器人，速度分为线速度v与角速度w，当对差动轮的速度有限制要求时，需要对进行限制，属于椭圆限制。

轮子驱动电机的速度限制定义

#define MOT_SPDMINRPM		(180.0f)
#define MOT_SPDMAXRPM		(1500.0f)
#define MOT1P_SPDMAXRPM		(MOT_SPDMAXRPM)
#define MOT2P_SPDMAXRPM		(MOT_SPDMAXRPM)

运动几何参数的定义

#define PARAM_WIDE_METER    (0.565)    // 两轮间距-宽度
#define PARAM_PULSE_PER_METER   (32595)  // 里程参数：每米脉冲数
#define PARAM_PULSE_LR_DIFF      (0.9997)  // 左右轮直径差：左轮比右轮
#define PARAM_PULSE1_PER_METER   (PARAM_PULSE_PER_METER*sqrt(PARAM_PULSE_LR_DIFF)) //左轮每米脉冲数
#define PARAM_PULSE2_PER_METER   (PARAM_PULSE_PER_METER/sqrt(PARAM_PULSE_LR_DIFF)) //右轮每米脉冲数 
#define ENC_RESOLUTION  (1024)    // 编码器分辨率

线速度与角速度的限制

#define VEL_LINE_X_MAX   (MOT_SPDMAXRPM/60.0*(double)ENC_RESOLUTION/(double)PARAM_PULSE_PER_METER) // (1.2)
#define VEL_ANGLE_Z_MAX     (1.0)

特别说明：在此，角速度最大值不取 VEL_LINE_X_MAX/(0.5*PARAM_WIDE_METER)，是因为此值过大，角速度过高，载人感受不舒服。

等效最大线速度
v_max_by_motor = VEL_LINE_X_MAX;
等效对大角速度
w_max_by_motor = v_max_by_motor/(0.5*PARAM_WIDE_METER);

第一种限制功能：

限制角速度与线速度不超出最大限制值；
声明变量与函数

double mot_min_spdrpm;
double v_max_by_motor;
double w_max_by_motor;
void vw_limit_max_by_motor(double &v, double &w);

函数定义

void WheelChair::vw_limit_max_by_motor(double &v, double &w)
{
	//　折算到轮子上的最大线速度
   float v_eq_max = fabsf(v) + fabsf(w)*0.5*PARAM_WIDE_METER;
   if (v_eq_max > v_max_by_motor)
   {
       double k = v_max_by_motor/v_eq_max;
       v = v*k;
       w = w*k;
   }
}

初始化

v_max_by_motor = VEL_LINE_X_MAX;
w_max_by_motor = v_max_by_motor/(0.5*PARAM_WIDE_METER);

调用

void WheelChair::car_set_wrist_spd(double v, double w)
{    
    vw_limit_max_by_motor(v, w);
    spd_linear = v;
    spd_angle = w;
}

第二种限制

限制速度不小于某阈值
声明变量与函数

double v_min_by_motor;
double w_min_by_motor;
void vw_out_min_by_motor(double &v, double &w);

定义函数

void WheelChair::vw_out_min_by_motor(double &v, double &w)
{
    // 当指令为0时的处理，主要是要考虑方向；
    if ((fabs(v) < 0.001)&&(fabs(w) < 0.001))
    {
        v = (odom.odom_vx > 0.0)? v_min_by_motor : -v_min_by_motor;
        w = 0.0;
        return;
    }
   double theta = atan2(w, v);
   double v_circle = v_min_by_motor*cos(theta); 
   double w_circle = w_min_by_motor*sin(theta);

   // 进入椭圆，则限制
   if (fabs(v_circle) > fabs(v))
   {
       v = v_circle;
       w = w_circle;
   }
}

变量初始化

 mot_min_spdrpm = MOT_SPDMINRPM;
    v_min_by_motor = (MOT_SPDMINRPM/60.0*(double)ENC_RESOLUTION/(double)PARAM_PULSE_PER_METER);
    w_min_by_motor = v_min_by_motor/(0.5*PARAM_WIDE_METER);

调用

double v_star = pObj->spd_linear, w_star = pObj->spd_angle;
pObj->vw_out_min_by_motor(v_star, w_star);  // 调用
pObj->vf = pObj->tramp_v.update(v_star, dt);
pObj->wf = pObj->tramp_w.update(w_star, dt);