[发明专利]一种四轮驱动全向底盘的控制装置及控制方法

权利要求书

1.一种四轮驱动全向底盘的控制装置，其特征在于包括由ARM和FPGA组成的主控模块，无线串口通信模块，码盘计数模块，直流电机控制模块；无线串口通信模块与ARM相连，设置于底盘的无线串口通信模块用来接收来自于计算机连接的主控模块处的无线串口通信模块的指令，指令包括四轮驱动全向底盘所要移动到的目标点的位置值以及姿态角值，将接收到的指令传给上位机ARM；FPGA与ARM相连，FPGA将计数电路和信息采集电路融合，由FPGA对码盘计数模块的数据进行采集与处理，然后通过通用I/0接口传输到ARM，同时FPGA也作为接口电路，将ARM的控制指令传输到直流电机控制模块，完成对直流电机的转速以及方向的控制；ARM是整个控制装置的核心控制模块，用以对接收到的数据进行融合，实现对四轮驱动全向底盘的位姿进行实时结算，并完成对四个直流电机的实时控制；

码盘计数模块与FPGA相连，当码盘轮转动1圈时，码盘计数模块会记录1200个数，因此通过码盘计数模块实现对码盘轮行走的距离进行记录，再通过码盘轮与驱动轮的半径之比，得到驱动轮的行走距离；电机控制模块与FPGA相连，其作用是对直流电机的电流环进行闭环控制；

按一定周期对车的轨迹微分，本周期初始时刻，车的位姿为已知的x1和y1和thetal，结束时车的位姿为未知的x2和y2和theta2；在这个周期内左轮的位移为dl，右轮的位移为dr，前轮的位移为dh，后轮的位移为db，由于四轮全向运动时各轮的侧滑严重，码盘记的位移只沿着各个轮的正方向，不会记录各轮侧滑而产生的位移，所以码盘记的各轮位移dl和dr和dh和db是表示轮子前进方向的各轮实际运动方向的分量，按以下公式解算车的质心的位移：

$d=\sqrt{{\left(\frac{dl+dr}{2}\right)}^2+{\left(\frac{dh+db}{2}\right)}^2}$

车转过的角度为：

$\mathrm{\Δ}\mathrm{\θ}=\frac{1}{2}(\frac{dr-dl}{L_{lr}}+\frac{dh-db}{L_{bh}})$

上式中，L_1r为车左右轮的码盘间距，L_bh为车前后轮的码盘间距；

由Δθ可以求得本周期结束时车的角度为：

theta2＝thetal+Δθ

当这一段位移足够小时，轨迹可以近似成一条与水平方向夹角为VelTheta的直线；其中：

Veltheta＝thetal+arctan((db+dh)/(dr+dl))

则有：

x₂＝x1+d*cos(Veltheta)

y₂＝y1+d*sin(Veltheta)

至此，在平面运动中，知道了机体的初始位姿，并记录了机体的各轮的位移，则四轮全向底盘在任意时刻的位姿都可以得到；

在四轮全向底盘进行直线运动时，需要给定目标的坐标点和直线与场地坐标系x轴正方向的夹角；机器人根据这条目标直线和当前自身的坐标值和航向角，判断与目标直线的位置和角度偏差，同时控制四轮差速修正行进轨迹；

在四轮全向底盘轨迹的修正过程中，需要调节的两个偏移量为横向偏差和航向偏差，当同时对这两个变量进行调节时，则根据机体在不同的情况给调节参数赋予不同的符号控制电机差速调节；

引入一个前置探测点，即在机体行进方向的前方某一位置上，放置一个虚拟点，计算此点与目标直线的偏差，并根据这个偏差设置反馈增益来计算修正的四个轮的差速值，修正机体行进的直线轨迹；前置探测点长短的选择和反馈增益系数需要根据机器人实际行走的调试效果来整定；

位置型PID控制算法的基本算式为：

$u\left(k\right)=K_p\{e\left(k\right)+K_I\underset{j=0}{\overset{k}{\Σ}}e\left(j\right)+K_D\[e\left(k\right)-e(k-1)\]\}$

增量型PID控制算法的基本算式为：

△u(k)＝K_p{[e(k)-e(k-1)]+K_Ie(k)+K_D[e(k)-2e(k-1)+e(k-2)]}

其中，u(k)——第k次采样时计算机输出值；e(k)——第k次采样时的偏差值；

Δu(k)——第k次采样与第k-1次采样的计算机的差值；

当执行部件不带积分部件，其位置与计算机输出的数字量是一一对应，采用位置式PID控制算法；当执行部件带积分部件时，选用增量式PID控制算法；

底盘的行进过程共有三个阶段，分别为加速阶段、匀速阶段和减速阶段；在每个阶段中都要实时计算与目标点的距离，若达到目标点就停止；在加速段中，可采用匀加速的控制方式，但首先需设定好一个最大速度，当速度达到最大速度时，则进入匀速区，进入匀速区后，需检测是否到达减速区，当进入减速区时，减速区减速的方式采用的是离目标距离与减速区长度之比乘以当前速度，得到一个平滑的衰减过程，最终的速度会缓慢的到达零；实际过程中，考虑到时间的要求以及防止电机低速转动，当速度减到10％的最大速度即以此匀速速度行走，直到到达目标点停止或者直接在程序上进入下一段路径。

2.一种四轮驱动全向底盘的控制方法，其特征在于具体步骤如下：

设定四轮驱动全向底盘所要移动到的目标点的位置值，即X、Y坐标，以及机体自身所

要转动的角度θ，启动开始按钮，通过无线串口通信方式，四轮驱动全向底盘上安装的主控模块接收到指令后，按照预先设定的四轮驱动全向底盘运动的控制算法，通过直流电机控制模块，控制四个直流电机协调工作，采用的导航方式为四码盘导航方式，通过码盘计数模块实时计数，可以记录下各轮行走的距离，将此数据传给主控模块，主控模块再对四个直流电机进行实时的PID控制，实时计算当前运动轨迹与预设轨迹之间的偏差，不断的进行偏差纠正，使整个机体沿预设的轨迹进行运动，实现在自动方向沿直线行走并且实时改变自身姿态角；

无线串口通信模块与ARM相连，用来接收来自于计算机连接的无线串口通信模块的指令，指令信息包括四轮驱动全向底盘所要移动到的目标点的位置值以及姿态角值，将接收到的指令传给上位机ARM；FPGA与ARM相连，将计数电路和信息采集电路融合，由FPGA对码盘计数模块的数据进行采集与处理，然后通过通用I/0接口传输到ARM，同时FPGA也作为接口电路，将ARM的控制指令传输到直流电机控制模块，完成对直流电机的转速以及方向的控制；ARM是整个控制装置的核心控制模块，用以对接收到的数据进行融合，实现对四轮驱动全向底盘的位姿进行实时结算，并完成对四个直流电机的实时控制；码盘计数模块与FPGA相连，当码盘轮转动1圈时，码盘计数模块会记录1200个数，因此可通过码盘计数模块实现对码盘轮行走的距离进行记录，再通过码盘轮与驱动轮的半径之比，则可得到驱动轮的行走距离；电机控制模块与FPGA相连，其作用是对直流电机的电流环进行闭环控制；

按一定周期对车的轨迹微分，本周期初始时刻，车的位姿为已知的x1和y1和thetal，结束时车的位姿为未知的x2和y2和theta2；在这个周期内左轮的位移为dl，右轮的位移为dr，前轮的位移为dh，后轮的位移为db，由于四轮全向运动时各轮的侧滑严重，码盘记的位移只沿着各个轮的正方向，不会记录各轮侧滑而产生的位移，所以码盘记的各轮位移dl和dr和dh和db是表示轮子前进方向的各轮实际运动方向的分量，按以下公式解算车的质心的位移：

$d=\sqrt{{\left(\frac{dl+dr}{2}\right)}^2+{\left(\frac{dh+db}{2}\right)}^2}$

车转过的角度为：

$\mathrm{\Δ}\mathrm{\θ}=\frac{1}{2}(\frac{dr-dl}{L_{lr}}+\frac{dh-db}{L_{bh}})$

上式中，L_1r为车左右轮的码盘间距，L_bh为车前后轮的码盘间距；

由Δθ可以求得本周期结束时车的角度为：

theta2＝thetal+Δθ

当这一段位移足够小时，轨迹可以近似成一条与水平方向夹角为VelTheta的直线；其中：

Veltheta＝thetal+arctan((db+dh)/(dr+dl))

则有：

x₂＝x1+d*cos(Veltheta)

y₂＝y1+d*sin(Veltheta)

至此，在平面运动中，知道了机体的初始位姿，并记录了机体的各轮的位移，则四轮全向底盘在任意时刻的位姿都可以得到；

在四轮全向底盘进行直线运动时，需要给定目标的坐标点和直线与场地坐标系x轴正方向的夹角；机器人根据这条目标直线和当前自身的坐标值和航向角，判断与目标直线的位置和角度偏差，同时控制四轮差速修正行进轨迹；

在四轮全向底盘轨迹的修正过程中，需要调节的两个偏移量为横向偏差和航向偏差，当同时对这两个变量进行调节，则根据机体在不同的情况给调节参数赋予不同的符号控制电机差速调节；

引入一个前置探测点，即在机体行进方向的前方某一位置上，放置一个虚拟点，计算此点与目标直线的偏差，并根据这个偏差设置反馈增益来计算修正的四个轮的差速值，修正机体行进的直线轨迹；前置探测点长短的选择和反馈增益系数需要根据机器人实际行走的调试效果来整定；

位置型PID控制算法的基本算式为：

$u\left(k\right)=K_p\{e\left(k\right)+K_I\underset{j=0}{\overset{k}{\Σ}}e\left(j\right)+K_D\[e\left(k\right)-e(k-1)\]\}$

增量型PID控制算法的基本算式为：

△u(k)＝K_p{[e(k)-e(k-1)]+K_Ie(k)+K_D[e(k)-2e(k-1)+e(k-2)]}

其中，u(k)——第k次采样时计算机输出值；e(k)——第k次采样时的偏差值；

Δu(k)——第k次采样与第k-1次采样的计算机的差值；

当执行部件不带积分部件，其位置与计算机输出的数字量是一一对应，采用位置式PID控制算法；当执行部件带积分部件时，选用增量式PID控制算法；

底盘的行进过程共有三个阶段，分别为加速阶段、匀速阶段和减速阶段；在每个阶段中都要实时计算与目标点的距离，若达到目标点就停止；在加速段中，可采用匀加速的控制方式，但首先需设定好一个最大速度，当速度达到最大速度时，则进入匀速区，进入匀速区后，需检测是否到达减速区，当进入减速区时，减速区减速的方式采用的是离目标距离与减速区长度之比乘以当前速度，得到一个平滑的衰减过程，最终的速度会缓慢的到达零；实际过程中，考虑到时间的要求以及防止电机低速转动，当速度减到10％的最大速度即以此匀速速度行走，直到到达目标点停止或者直接在程序上进入下一段路径。