[发明专利]一种基于两轮差速机器人的运动控制方法

权利要求书

1.一种基于两轮差速机器人的运动控制方法，其特征在于，具体步骤为：

步骤1、对两轮差速机器人进行运动学分析，创建其运动模型，具体步骤为：

步骤1-1、测量得到机器人两个驱动轮之间的间距L，测量得到驱动轮半径r；

步骤1-2、设定逆时针方向为正方向，构建两轮差速机器人运动模型，具体为：

其中，v_x是机器人在环境坐标系下沿x轴方向的速度，v_y是机器人在环境坐标系下沿y轴方向的速度，θ为机体坐标系相对于环境坐标系的偏转角，ω_r是机器人右侧驱动轮的角速度，ω_l是机器人左侧驱动轮的角速度；

步骤2、制定两轮差速机器人的运动控制方案，具体是确定输入左右轮驱动电机的PWM值与控制量之间的转换关系，所述控制量包括角度环控制量u_θ、速度环控制量u_v，具体步骤为：

步骤2-1、使用两个驱动电机对左右轮分别进行独立驱动；

步骤2-2、设定右轮电机驱动模块的PWM值为T_r：

T_r＝K·(u_v+u_θ)

其中，K为控制量到电机驱动模块的PWM值的转换系数，u_v为速度环控制量，u_θ为角度环控制量；

步骤2-3、设定左轮电机驱动模块的PWM值为T_L：

T_l＝K·(u_v-u_θ)

步骤3、创建两轮差速机器人相对于期望运动轨迹的矢量场，具体步骤为：

步骤3-1、将机器人期望运动轨迹处理为轨迹坐标以及期望速度关于时间的函数，并计算当前时刻期望运动轨迹的坐标以及期望速度的大小和方向；

步骤3-2、构建矢量场函数：

χ^d(y)＝-χ^∞+tan^-1(k·y)

其中，y表示机器人当前位置距离当前时刻期望运动轨迹的侧偏距离，χ^d为目标航向值，χ^∞是y无穷大时的目标航向角，k为正整数；

步骤4、设计偏转角PID控制器，求出角度环控制量u_θ，该偏转角PID控制器以角度误差e_θ为输入，以角度环控制量u_θ为输出，具体步骤为：

步骤4-1、测量当前时刻机器人在地图坐标系下的位置坐标以及实际航向角度；

步骤4-2、将机器人位置坐标代入到步骤3中构建的矢量场中，求出当前时刻的期望航向角度；

步骤4-3、将机器人实际航向角度与期望航向角度作差求得角度误差e_θ；

步骤4-4、求解出角度环控制量u_θ：

其中，K_pθ为角度环比例项系数，K_iθ为角度环积分项系数，K_dθ为角度环微分项系数；

步骤5、设计速度PID控制器，求出速度环控制量u_v，该速度PID控制器以速度误差e_v为输入，以速度环控制量u_v为输出，具体步骤为：

步骤5-1、测量机器人当前实际速度；

步骤5-2、将机器人当前实际速度与期望速度作差求得速度误差e_v；

步骤5-3、求解出速度环控制量u_v：

其中，K_pv为速度环比例项系数，K_iv为速度环积分项系数，K_dv为速度环微分项系数；

步骤6、计算得到最终需要输入电机驱动模块的PWM值，并将其输入电机驱动模块。