一种用于目标跟踪的全向移动平台系统及控制方法

摘要

本发明提供一种用于目标跟踪的全向移动平台系统及控制方法，包括底盘，底盘前后部对称安装麦克纳姆轮，麦克纳姆轮由伺服电机驱动，伺服电机通过电机驱动控制器与中央处理器连接并通信，云台旋转安装在底盘上，云台搭载设置中央处理器、九轴陀螺仪传感器，九轴陀螺仪传感器、云台电机的驱动控制器与中央处理器线路连接并通信，中央处理器，用于处理九轴陀螺仪传感器和麦克纳姆轮、云台的电机驱动控制器反馈的数据，驱动平台移动。该系统具有小型化、数字化和集成化的特点，旨在为复杂环境中移动的机械设备提供一个稳定的视场，或使设备以预定的规律移动排除其他因素的干扰。

说明书

技术领域

本发明属于控制技术领域，具体涉及一种用于目标跟踪的全向移动平台系统及控制方法。

背景技术

在目前机器人、车船、军事等领域的研究中，主要攻克的便是环境适应能力，在面对复杂崎岖的路面、沟壑或者海面时，利用雷达、单目或者双目摄像头，以及其他传感器，会出现雷达天线体积庞大无法规律运转、摄像头无法稳定、传感器数据采集错位等等问题，以至于无法采集稳定的环境数据而导致运动效率低下，难以做到精准的控制，无法达到目标效果。

在这个领域研究中，一般会涉及到PID控制算法、Kalman滤波算法、浮点运算单元和数字信号处理等算法，但是一般技术依然无法满足实际使用需要，例如公开号为CN110209186A的带漂移补偿的陀螺稳定控制系统，公开号为CN108519087A的基于MEMS陀螺的跟踪平台装置及跟踪方法，依然不能达到满意的效果。

发明内容

本发明的目的在于提供一种用于目标跟踪的全向移动平台系统，该系统具有小型化、数字化和集成化的特点，旨在为复杂环境中移动的机械设备提供一个稳定的视场，或使设备以预定的规律移动排除其他因素的干扰。

提供该移动平台的控制方法是本发明的另一个目的。

为实现上述目的，本发明的技术方案是：一种用于目标跟踪的全向移动平台系统，包括底盘，一组麦克纳姆轮，云台，中央处理器，九轴陀螺仪传感器，

所述底盘前后部对称安装麦克纳姆轮，麦克纳姆轮由伺服电机驱动，伺服电机通过电机驱动控制器与中央处理器连接并通信，

所述云台旋转安装在底盘上，云台搭载设置中央处理器、九轴陀螺仪传感器，所述九轴陀螺仪传感器、云台电机的驱动控制器与中央处理器线路连接并通信，用于将采集的方位角，俯仰角，位置速度信息反馈给中央处理器处理、存储；

所述中央处理器，用于处理九轴陀螺仪传感器和麦克纳姆轮、云台的电机驱动控制器反馈的数据，驱动平台移动。

所述底盘同侧的麦克纳姆轮互为镜像关系。

所述云台通过导电滑环安装在底盘上。

所述电机驱动控制器与中央处理器通过CAN总线通信方式连接，所述九轴陀螺仪传感器与中央处理器通过串口通信方式连接。

所述云台包括pitch轴、yaw轴。

本发明全向移动平台系统具有小型化、数字化和集成化的特点，可以为机械设备在复杂环境中的运行提供稳定的视场。

一种用于目标跟踪的全向移动平台系统的控制方法，

1、搭建实验平台，平台包括四角对称安装有麦克纳姆轮的底盘，麦克纳姆轮由伺服电机驱动，同侧麦克纳姆轮互为镜像关系；底盘上旋转设置云台，所述云台包括pitch轴、yaw轴；云台搭载中央处理器、九轴陀螺仪传感器，所述九轴陀螺仪传感器、云台的电机驱动控制器、底盘麦克纳姆轮的电机驱动控制器与中央处理器线路连接，实现对云台的方位角，俯仰角，位置速度信息及电机驱动控制器反馈得到的底盘数据的实时数据收集、存储；

2、将底盘麦克纳姆轮伺服电机驱动控制器反馈得到的底盘数据，结合PID控制算法，从而控制电机驱动控制器的驱动电流，做出底盘PID速度闭环，配合麦克纳姆轮进行全向移动；

3、将云台电机驱动控制器、九轴陀螺仪传感器反馈得到的云台数据，结合模糊PID控制算法，从而控制电机驱动控制器的驱动电流，做出云台的位置速度串级PID双环，完成基础移动；

4、运用Kalman滤波算法，对使用者控制的目的坐标或者摄像头反馈的控制目的坐标的离散化数据集，进行滤波，防止坐标跳变以及做出目标预测，将得到的数据反馈给中央处理器；

5、结合步骤1、步骤5得到的数据，利用中央处理器自带的DSP库函数，通过算法计算出底盘、云台的实时位置，并作出精准的控制，从而实现稳定移动跟踪平台；

算法如下：

a、以平台中心为极点，云台的初始方向作为极轴，取顺时针方向为正，长度单位为云台yaw轴反馈的速度，建立平面极坐标系，极坐标系内任意一点（ρ，θ）代表某时刻云台与底盘的相对速度和相对位置；

b、记录云台yaw轴电机转子初始位置

计算总角度

相对位置：

c、分别以平台底盘、平台云台为中心，自定义向前为y轴正方向，向右为x轴正方向，建立两个平面直角坐标系

d、解算目标位置，在平面直角坐标系

其中：

且目标速度

e、即可在平面直角坐标系

将解算数据带入麦克纳姆轮底盘解算

即

其中

再将底盘电机的目标数据带入伺服电机的伺服控制器中；

6、利用步骤a采集到的九轴陀螺仪传感器数据，解算得到欧拉角作为云台的绝对位置信息，机器人云台采用速度、位置串级PID双闭环控制，实现对云台的绝对控制。

本发明移动跟踪平台，利用Kalman滤波算法获取目标值、利用九轴陀螺仪传感器来获取到平台的姿态，再结合PID控制器以及数字信号处理技术对电机进行伺服控制，有效的保证了平台视场的稳定性、时效性和精准度。

附图说明

图1为 本发明的结构示意图。

图2为图1的左视图。

图3为图 的俯视图。

图4为本发明的硬件框图。

图5为本发明平面直角坐标系

图6为本发明平面极坐标系示意图。

具体实施方式

如图1、6所示，一种用于目标跟踪的全向移动平台系统，包括底盘1，一组四个麦克纳姆轮2，云台3，中央处理器，九轴陀螺仪传感器，

底盘1前后部对称安装麦克纳姆轮2，麦克纳姆轮由伺服电机驱动，底盘同侧的麦克纳姆轮互为镜像关系，伺服电机通过电机驱动控制器与中央处理器通过CAN总线连接并通讯，

云台3通过导电滑环安装在底盘1上，云台包括pitch轴、yaw轴，云台搭载设置中央处理器、九轴陀螺仪传感器，所述九轴陀螺仪传感器与中央处理器通过串口通信方式连接，云台的电机驱动控制器与中央处理器通过CAN总线连接并通讯，用于将采集的方位角，俯仰角，位置速度信息反馈给中央处理器处理、存储；

中央处理器，用于处理九轴陀螺仪传感器和麦克纳姆轮、云台的电机驱动控制器反馈的数据，驱动平台移动

上述系统的控制方法是：

1、搭建实验平台，底盘由4个一组麦克纳姆轮组成，底盘一侧的两个麦克纳姆轮有着互为镜像的关系,即两轮可以完成相反的运动方式，利用速度的正交分解，两轮便可以分解成全向和自旋的速度分量。云台由pitch轴和yaw轴2个轴组成；云台搭载STM32开发板作为中央处理器，芯片型号为STM32F407VGT6,拥有丰富的扩展接口和通信接口，外设有九轴陀螺仪传感器，并将采集的方位角，俯仰角，位置速度信息反馈给中央处理器，经过数字信号处理后存储；云台底盘间利用导电滑环进行连接，实现云台的360°旋转；利用CAN总线通信方式，获取云台以及底盘电机编码器的实时数据，并存储。

2、将底盘电机编码器反馈得到的底盘数据，结合PID控制算法，从而控制电机驱动控制器的驱动电流，做出底盘PID速度闭环，配合麦克纳姆轮进行全向移动。

3、将云台电机编码器和九轴陀螺仪加速度传感器反馈得到的云台数据，结合模糊PID控制算法，从而控制电机驱动控制器的驱动电流，做出云台的位置速度串级PID双环，完成基础移动。

4、运用Kalman滤波算法，对使用者控制的目的坐标或者摄像头反馈的控制目的坐标的离散化数据集，进行滤波，防止坐标跳变以及做出目标预测，将得到的数据反馈给中央处理器。

5、结合步骤4、步骤7得到的数据，利用STM32F4自带的DSP库函数，通过算法计算出底盘、云台的实时位置，并作出精准的控制，从而实现稳定移动跟踪平台。

算法推导：

（1）以平台中心为极点，云台的初始方向作为极轴，取顺时针方向为正，长度单位为平台底盘旋转的角速度，即云台yaw轴反馈的速度，建立平面极坐标系。极坐标系内任意一点（ρ，θ）代表某时刻云台与底盘的相对速度和相对位置；

（2）记录云台yaw轴电机转子初始位置

计算总角度

相对位置：

（3）分别以平台底盘、平台云台为中心，自定义向前为y轴正方向，向右为x轴正方向，建立两个平面直角坐标系

（4）解算目标位置，在平面直角坐标系

其中：

且目标速度

（5）即可在平面直角坐标系

将解算数据带入麦克纳姆轮底盘解算，

即

其中

再将底盘电机的目标数据带入伺服电机的伺服控制器中；

6、利用步骤二采集到的陀螺仪数据，解算得到欧拉角作为云台的绝对位置信息，机器人云台采用速度、位置串级PID双闭环控制，即通过位置外环控制器，求出控制输出量，作为速度内环的控制目标值，实现对云台的绝对控制。