运动底盘共有4个轮子(A、B、C、D),4个轮子呈中心对称,其中AD、BC、AB、DC互为手性对称,每个麦轮由一组独立的电机驱动,4组电机都是带编码器的无刷直流电机。运动学分析图中,参数含义如下:
- W :轮距,底盘左右麦轮的距离,单位:m。
- H:轴距,底盘前后麦轮的距离,单位:m。
- Vy :底盘前后移动速度,前进为正,单位:m/s。
- Vx :底盘左右移动速度,左移为正,单位:m/s。
- Vz :底盘绕O点旋转速度,逆时针为正,单位:rad/s。(O点为轮距、轴距中心线的交点)
- VA轮、VB轮、VC轮、VD轮 :麦轮A、B、C、D的线速度,由电机转动带动麦轮轮毂产生,前进为正,单位:m/s。
所谓运动学分析,也就是分析Vx、Vy、Vz 与VA轮、VB轮、VC轮、VD轮 的关系。在分析演算过程中,涉及到每个轮子的相关参数,以下C轮为例,解释参数含义,A、B、D轮同理:
- VC辊 :麦轮C与地面接触的辊子的线速度,由麦轮C与地面的相对滑动产生(注意是滑动,不是麦轮C的转动产生的VC轮),垂直辊轴向前为正,单位:m/s。
- Vz_C :麦轮C质心绕点O旋转的线速度,正方向垂直于麦轮C质心与O点的连线,偏逆时针方向。
- VC_x :麦轮C质心的前后移动速度,与底盘前后移动速度Vx和底盘绕O点旋转速度Vz相关,前进为正,单位:m/s。
- VC_y :麦轮C质心的左右移动速度,与底盘左右移动速度Vx和底盘绕O点旋转速度Vz相关,左移为正,单位:m/s。
- αC :轮子C质心与O点的连线与前进方向的夹角,其值已知为tan?1 。
- β:轮轴和辊轴之间的夹角,为45°。
了解相关参数后,接下来分析这些参数之间的关系,以及如何求出四轮麦轮底盘的运动学正逆解公式。以麦轮C为例,求麦轮C质心的前后左右移动速度VC_x、VC_y与底盘整体的前后左右移动速度、绕O点旋转速度Vx、Vy、Vz之间的关系,底盘整体与4个麦轮质心可以认为是一个刚体,则速度分解可得:
[attach]4418[/attach]
式1、式2是麦轮C质心速度与底盘整体速度的关系,而麦轮C质心速度从根本上来说,还是靠VC轮和VC辊合并产生的。
已知VC轮是麦轮C的线速度,是由电机转动带动麦轮轮毂产生的。而VC辊是由麦轮C与地面的相对滑动产生的,VC辊速度的大小及方向是由麦轮A、B、C、D的差速和机械结构决定的,其动力来源也是电机,所以当底盘的运动速度需要VC辊辊转动时,就需要电机产生更大的扭矩,带动麦轮轮毂进而带动麦轮辊子。通过速度分解可得:
VC_x = VC轮 + VC辊 × sinβ 式3
VC_y = -VC辊 × cosβ 式4
联立式1、式3和式2、式4可得:
[attach]4422[/attach]
由式6,推导出VC辊的关系式:
[attach]4421[/attach]
将式7代入式5,设[attach]4423[/attach]可得:
VC轮- Vy - Vz × L ×cos αC ×tan β = Vx + Vz + L × sin αC
VC轮 = Vx + Vy + Vz × L ×(sin αC + cos αC + tan β)
其中β= 45°,则:
VC轮 = Vx + Vy + Vz × L ×(sin αC + cos αC)
又因为[attach]4426[/attach]则VC轮与Vx、Vy、Vz的关系:
[attach]4427[/attach]
同理,推导VA轮、VB轮、VD轮与Vx、Vy、Vz的关系,推导时需要注意A、B、D三个麦轮与地面接触的辊子的线速度方向,观察底盘运动学分析图容易知道,线速度方向影响的是麦轮C质心的左右移动速度:
VA_y = -VA辊 × cosβ,VB_y = -VB辊 × cosβ,VD_y = -VD辊 × cosβ
最后可以得底盘的运动学逆解公式,由三轴目标速度求出四个轮子的目标速度:
[attach]4428[/attach]
联立式8、式9、式10、式11,可以求出底盘的运动学正解公式,由四个轮子的实时速度求出三轴实时速度:
[attach]4429[/attach]
根据上述推导得到的数学公式,在运动底盘中,对应的运动学正解代码段:
- CarDat.Speed.X = (MOTOR_A.Encoder+MOTOR_B.Encoder+MOTOR_C.Encoder+MOTOR_D.Encoder)/4;
- CarDat.Speed.Y = (MOTOR_A.Encoder-MOTOR_B.Encoder+MOTOR_C.Encoder-MOTOR_D.Encoder)/4;
- CarDat.Speed.Z = (-MOTOR_A.Encoder-MOTOR_B.Encoder+MOTOR_C.Encoder+MOTOR_D.Encoder)/4/(Axle_spacing+Wheel_spacing);
复制代码其中,入口参数MOTOR_A.Encoder、MOTOR_B.Encoder、MOTOR_C.Encoder、MOTOR_D.Encoder为4个轮子的实时输出速度;
CarDat.Speed.X、CarDat.Speed.Y、CarDat.Speed.Z为底盘运动的三轴目标速度(Vz方向的速度,要除去底盘自身的尺寸);
Axle_spacing为小车(前后)轴距的1/2;
Wheel_spacing 为小车(左右)轮距的1/2。
对应的运动逆解代码段:
- Void Drive_Motor(float Vx, float Vy, float Vz)
- {
- MOTOR_A.Target = +Vy + Vx - Vz * (Axle_spacing + Wheel_spacing);
- MOTOR_B.Target = -Vy + Vx - Vz * (Axle_spacing + Wheel_spacing);
- MOTOR_C.Target = +Vy + Vx + Vz * (Axle_spacing + Wheel_spacing);
- MOTOR_D.Target = -Vy + Vx + Vz * (Axle_spacing + Wheel_spacing);
- }
复制代码其中,入口参数Vx、Vy、Vz为底盘运动的三轴目标速度;
MOTOR_A.Target、MOTOR_B.Target、MOTOR_C.Target、MOTOR_D.Target为4个轮子的实时输出速度;
Axle_spacing为小车(前后)轴距的1/2;
Wheel_spacing 为小车(左右)轮距的1/2。