八.四轮车驱动开发之一:正/逆向运动学分析

        最近一直在做ROS层的开发工作,也经常思考驱动控制板和ROS层如何配合和互相影响,因为以前做过ARM/STM32驱动层的整体开发,所以想从整体系统角度写一些关于驱动控制板的知识,一是对过去工作的温故知新;二是想系统梳理下整个系统框架,避免”只缘身在此山中”的遭遇.

我把这些内容组织成一个系列”四轮车驱动控制”,分多个小节来介绍:

1. 八.四轮车驱动开发之一:正/逆向运动学分析

2. 九.四轮车驱动开发之二: 配置PWM驱动直流电机

3. 十.四轮车驱动开发之三: 巧用编码器获取电机转速信息

4. 十一.四轮车驱动开发之四: 理解直流电机PID控制器

5. 十二.四轮车驱动开发之五: 由浅至深理解6轴陀螺仪姿态解算算法

        

        本节将从四轮车运动学方面分析四轮车的正向/逆向运动学方程.鉴于真实的机动车辆的运动学方程更为复杂,简单起见,这里仅局限于四轮车模型车辆做分析.

        我们知道传统的小型汽车都是四轮结构,总体可分两驱方式和四驱方式,而两驱方式又分为两驱前驱和两驱后驱方式. 不同的驱动结构,不同的车辆架构,其运动学分析方程大致相同但又有差异.

        以两驱前驱四轮车型为例,直线行驶时,前轮提供前进驱动动力,后轮只是被动跟随转动,前后四轮转速相同,不存在轮间差速现象(因路面状况原因除外). 车辆左右转弯时,前轮提供转向和动力,并由前驱差速器分配不同转速,转弯过程中前内侧轮转速低于前外侧轮,速度差与转弯角度成比例. 此时后轮左右两轮根据转弯角度也会有被动的差速现象,但两个后轮实际转速取决于车辆结构,需要根据运动学方程来计算. 有人会问既然后轮是被动跟随轮,为什么要计算它的转速? 现在的车辆上都有ESP,ABS等一些安全配置,这些安全配置都依赖于对车轮的期望转速和实际转速的分析,来做出不同的判断和操作,确保车辆安全性,稳定性. 当然作为四轮车模型,我们可以不考虑这些.

        传统两驱后驱或四驱汽车,后轮使用机械差速器完成差速配比,机械差速器的功能是:无论转弯或者直行,两侧驱动车轮的转速之和始终等于差速器壳转速的2倍(这里转速是角速度),或者说两后轮转速之和始终等于车速的2倍(这里速度是地面位移速度).比如整体车速为V, 左侧车轮转速为V-x,右侧车轮转速为V+x.

        对于电动四轮车来说,都是使用两个电机直接驱动左右后轮,这就不再需要机械差速, 只需要通过电控手段控制驱动电机转速来实现左右轮转速的配比(也就是后面章节将要将的PID控制器).

         再说说我使用的四轮车的前轮转向系统,前轮采用阿克曼转向系统,由程序提供转向角驱动转向舵机控制阿克曼系统实现转向,原理如下图所示:

 一. 逆向运动学分析

        逆向运动学分析的目的是提供车身期望的速度v和转向角度θ,计算出左右后轮的速度v1和v2.

        我们假设舵机控制转角为θ ， 四轮车期望前进速度为 v， 前后轮轴距(wheelbase)为 W， 后轮轮距(track)为 T，车辆直线前进或转弯时左侧轮速度为 v1 ,右侧轮速度为 v2 ；当转向时后轮质心到转向圆心的距离为R,即转向圆弧的半径. 有如下图选择后轮中心为小车质心:

注: 上图角度θ为小车转向角度或舵机转向角度,不等于前面左右轮的实际转向角度.

当四轮车以一定的速度转向时,其在瞬时时刻的转向方向就是其转向圆弧的切线,我们很容易得到:

        前轮中心到转向圆弧圆心的直线相对于后轮中心到圆心的直线的夹角就等于期望装向: θ.  如图所示.

        根据物理学中角速度的一致性,可知:  后轮中心的角速度 = 左轮角速度 = 右轮角速度

有:

        

进一步:     

 

进一步可得:

        上面两个公式就是我们最终的逆向运动学方程, 我们只要给小车一个总的期望速度v和转向角度θ,就可以经这两个公式计算出需要分配给左右后轮的实际速度v1和v2. 实现了类似于机械差速锁一样的功能. 

        当然这里只是求出期望的速度,具体到控制转速,还需要把期望速度通过PID控制器求出PWM去驱动左右电机转动(这个后面小节会继续介绍).

二. 正向向运动学分析

      上面分析逆向运动学是为了给出转向角度和整体速度,去控制四轮车的运动(直行或转向); 而正向运动学分析的目的与逆向运动学大致相反, 是根据两个后轮驱动电机的编码器值,求出车辆的实际速度v,实际位移d及偏航角α.

        正向运动学分析需要两个坐标系: 地图坐标系和载体坐标系. 如果逆向运动学是在载体坐标系控制小车移动的过程,那么正向运动学就是在地图坐标系测量小车位姿的过程.最终正向运动学的测量结果就是在X-Y-Z三轴的地图坐标(x,y,z)和地图坐标中三轴的欧拉角(roll,pitch,yaw)(实际只用了偏航角yaw).

        看过我前面关于扩展卡尔曼滤波器robot_pose_ekf介绍,应该知道,robot_pose_ekf就是融合车轮的里程计(odom)数据(x,y,z,roll,pitch,yaw)和IMU(惯性传感器,就是6轴陀螺仪)的数据(roll,pitch,yaw).   这里轮式里程计(odom)就是来自正向运动学分析的结果.

如下图:  小车在地图坐标系(X-Y-Z)中移动,移动偏航角(yaw)为α,偏航角瞬时变化为Δα,速度为v,位移位d. 小车在自身载体坐标系(X`-Y`-Z`)中(以后轮中心为质心),按IMU的x`轴为前进方向,右侧为IMU的y`轴方向,转向角为θ,在x`方向位移瞬时变化为Δx',在y`方向位移瞬时变化为Δy'.

1.从驱动程序可以获取左右轮驱动电机的编码器值decode1和decode2,经过计算可以得到左右轮实际速度v1和v2,以及左右轮瞬时位移Δd1和Δd2,也可以称为瞬时速度: Δv1和Δv2. 

2. 我们知道任何时刻两后轮位移之和始终等于整车位移的2倍,可以推理出任何时刻两后轮位移之和始终等于整车位移的2倍.于是我们可以得到整车前进方向(X`轴)的瞬时位移:Δx'=(Δd1+Δd2)/2.

3.根据记录的当前偏航角α,由已得的整车前进位移Δx',可以计算出小车在地图坐标系中,在X轴和Y轴的累计位移:

x = x + Δx'cosα

y = y + Δx'sinα

这里的x,y就是小车在地图坐标系中的实际坐标位置(x,y,z=0), 以内不考虑垂直方向移动,所以z=0.

4. 根据整体前后轮中心速度的一致性,我们的前轮中心在载体坐标系(X`-Y`-Z`)中瞬时位移也是Δx',我们可以从转向舵机的状态获取相当装向角θ,(因为阿克曼转向机械原理,前左右轮实际转向并不完全等θ). 我们可以很容易求得整体小车在载体坐标系y`轴上的瞬时位移: Δy' = Δx' tanθ.

5.开头介绍,我们的正向运动学分析基于小车后左右轮中心为质心,那么以前轮中心计算的Δy'必然会引起整个小车在地图坐标系中的偏航角α的变化: Δα.

由图可得:  tan(Δα) = Δy' / W.   当Δy'足够小时, Δα = Δy'/W.

所以有,地图坐标系中,小车的瞬时角速度为: Δα = Δy' / W = Δx' tanθ / W

进一步可得,整车在地图坐标系(X-Y-Z)中偏航角(yaw)为:

α  = α + Δα

yaw = α

到此, 我们得到了一个完整的小车位姿信息:

(x, y, z=0, roll=0, pitch=0, yaw) 

这些信息已经足够小车提供轮式里程计(ODOM)所用, 进一步和IMU的数据融合,计算出最优位姿估计.

如果想获取更多信息,比如整体车身在载体坐标系X'轴方向的瞬时速度: v' = Δx'

车身的瞬时偏航角速度: α`=  Δα.

(x, y, z=0, roll=0, pitch=0, yaw, v',  α`) 

三. 从ROS系统和驱动控制板整体,看运动学分析方程在整个系统中位置: