专利名称：角位置无线随动控制装置及其方法
技术领域：
本发明涉及精密机械运动控制领域，特别涉及一种角位置无线随动控制装置及其实现方法。
背景技术：
角位置随动是指发送机旋转时，接收机随之同步旋转，两者的旋转角度与位置一致。通过自整角机的自整步作用使接收机与发送机同步旋转，实现角度的随动控制。当今运动控制领域中，角位置在很多领域得到了广泛的应用，在军事领域中，火炮的主从随动系统实现了目标的集中打击。无人侦察、战斗平台等方面，为了减少伤亡，往往也需要等控制系统中有重要的应用。在工业领域中，往往很多精密操作以及工作环境，人类无法亲自动手操作，这时角位置随动系统就起到了至关重要的作用，目前，在角位置随动控制中，发送机和接收机多采用自整角机来实现，两台自整角机之间通过电缆连接来传递控制信号。当发送端和接收端距离较远，且在现场布线困难的特殊场合下，角位置随动控制的应用为往往会具有非常大的困扰，不能满足当今远程化、高机动性的要求。当发送端和接收端距离较远，且在现场布线困难的特殊场合下，如何实现发送端和接收端之间控制信号传递，并准确、可靠地实现两端的角位置同步运动是一个急需解决的问题。
发明内容
本发明的目的克服上述角位置随动系统的不足，提供一种角位置无线随动装置及其方法，从而实现无线随动控制，指令控制和视频监控。本发明提供的一种角位置无线随动控制装置及其方法，其特征在于，包括安装在被随动物体上或者监控室的上位机模块；以及，安装于应用现场，具有伺服电机的下位机模块；其中，上位机模块具有数据采集部，采集被跟随物体的转动角速度，并将产生相对应的角速度信号；上位机控制部，将角速度信号进行处理，转换成相对应的控制信号；上位机通信部，将控制信号送至下位机模块；下位机模块具有下位机通信部，接收控制信号；控制驱动部，对伺服电机进行零位置校准，在完成零位置校准之后，根据控制信号驱动伺服电机，完成角位置同步运动。进一步，本发明的角位置无线随动控制装置，还可以具有这样的特征其中，上位机模块还具有显示部，下位机还具有视频采集部；视频采集部，通过摄像头，采集伺服电机的运动状况，并转换成相应的视频信号;
下位机通信部还将视频信号送出至上位机通信部；显示部，根据上位机通信部接收到的视频信息，显示运动状况。进一步，本发明的角位置无线随动控制装置，还可以具有这样的特征其中，上位机控制部还具有指令输入部，供用户输入控制指令，并将控制指令转换成相应的指令信号，传输给下位机模块，控制伺服电机运动。进一步，本发明的角位置无线随动控制装置，还可以具有这样的特征其中，控制驱动部通过激光对射模块对伺服电机进行零位置校准，激光对射模块先检测伺服电机是否停留在零点位置，即激光对射模块的安装位置；若不在，则驱动伺服电机至零点位置，并发出复位命令。进一步，本发明的角位置无线随动控制装置，还可以具有这样的特征本发明的角位置无线随动控制装置的角位置控制方法，包括如下步骤( I)数据采集部实时获取被测物体的转动角速度；(2)上位机控制部对转动角速度进行软件滤波、数据打包、数据分析后形成控制信号;(3)上位机通信部将控制信号传输至下位机模块；(4)下位机控制部先将伺服电机进行零位置校准，然后根据控制信号，驱动伺服电机进行角位置同步运动。(5)视频采集部将伺服电机的运动状况发送至显示部，供用户实时观察伺服电机的运动状况。进一步，本发明的角位置无线随动控制装置，还可以具有这样的特征其中，用户可跳过步骤(I)、步骤(2)，直接在控制部直接输入指令，在控制部将指令转换成指令控制信号后继续步骤(3广步骤(5)，完成指令控制伺服电机进行角位置同步运动。发明作用与效果本发明的角位置无线随动控制装置及其方法，可实现无线随动控制，指令控制和视频监控，使得角位置随动系统使用灵活、应用范围广泛。且本发明传输稳定，保障了本发明在多个工作领域下的稳定性和实用性。
图1是本发明的角位置无线随动控制装置的结构原理图；图2是本发明的角位置无线随动控制方法的流程图；图3是本发明的上位机ARM主程序流程图；图4是本发明的下位机ARM主程序流程图；图5是本发明的监控程序运行界面；图6是本发明的跟随角度和误差对比图；图7是本发明的角速度采集图(降低误差前)；图8是本发明的角速度采集图(降低误差后)。
具体实施方式
本实施例以MEMS陀螺仪，ARM控制器，伺服电机，伺服电机驱动器对本发明的角位置无线随动控制装置及其方法进行详细说明。但本实施例并不限定本发明的范围。图1是本发明的角位置无线随动控制装置的结构原理图；图2是本发明的角位置无线随动控制方法的流程图；图3是本发明的上位机ARM主程序流程图；图4是本发明的下位机ARM主程序流程图；图5是本发明的监控程序运行界面；图6是本发明的跟随角度和误差对比图；图7是本发明的角速度采集图(降低误差前)；图8是本发明的角速度采集图(降低误差后)。本发明提供的一种角位置无线随动控制装置及其方法，其特征在于，包括安装在被随动物体上或者监控室的上位机模块11;以及，安装于应用现场，具有伺服电机的下位机模块12 ；其中，上位机模块11具有数据采集部111，通过MEMS陀螺仪采集被跟随物体的转动角速度，并将产生相对应的角速度信号；上位机控制部112，使用上位机ARM控制器将角速度信号进行处理，转换成相对应的控制信号；上位机通信部113，具有2. 4GHZ无线数据模块1131和1. 2GHZ无线视频模块1132，通过无线数据模块1231将控制信号送至下位机模块12 ；下位机模块具12具有下位机通信部121，由2. 4GHZ无线数据模块1211和1. 2GHZ无线视频模块1212，通过无线数据模块1211接收控制信号；控制驱动部122，通过激光对射模块1222对伺服电机1224进行零位置校准，激光对射模块先检测伺服电机是否停留在零点位置，即激光对射模块的安装位置；若不在，则驱动伺服电机至零点位置，并发出复位命令。在完成零位置校准之后，下位机ARM控制器1221根据控制信号通过直流伺服电机驱动器1223驱动伺服电机1224，完成角位置同步运动。上位机模块还具有显示部114，下位机还具有视频采集部123 ；视频采集部123，通过摄像头1232，采集伺服电机1224的运动状况，并转换成相应的视频信号；下位机通信部的无线视频模块1212将视频信号送出至上位机通信部的无线视频模块1132 ;显示部114，根据上位机通信部接收到的视频信息，通过PC机显示伺服电机运动状况。上位机控制部的ARM控制器还具有指令输入部，供用户输入控制指令，并将控制指令转换成相应的指令信号，传输给下位机模块，控制伺服电机运动。如图2所示，本发明的角位置无线随动控制装置的角位置控制方法，包括如下步骤(I)系统上电；
(2) MEMS陀螺仪实时获取被测物体的转动角速度；(3)上位机ARM控制器对转动角速度进行软件滤波、数据打包、数据分析后形成控制信号；(4)上位机通信部的无线数据模块将控制信号传输至下位机模块；(5)下位机通信部的无线数据模块接收控制信号；(6)下位机控制部的ARM控制器判断伺服电机是否在零位置，若不在，则通过激光对射模块对伺服电机进行零位置校准；(7)校准完后，ARM控制器根据控制信号，驱动伺服电机进行角位置同步运动。( 8 )视频采集部通过摄像头将伺服电机的运动状况通过下位机通信部的无线视频模块发送至上位机通信部的无线视频模块，再至显示部，供用户实时观察伺服电机的运动状况。用户可跳过步骤(2)、步骤(3)，直接在控制部直接输入指令，在控制部将指令转换成指令控制信号后继续后续步骤，完成指令控制伺服电机进行角位置同步运动。本实施例采用Keil MDK和VB 6. O开发平台，Keil MDK编写上/下位机ARM处理器程序，VB 6. O编写PC机的视频监控程序。软件设计主要包括视频监控程序、控制器上位机程序和控制器下位机程序，主要涉及到8个模块的编程SP1、L3G4200D、NRF24L01、STM32、IXD、串口中断处理、零位自检和监控软件等。激光对射模块的对伺服电机的校准如下伺服电机零位置是电机的起始位置，它与被随动物体的起始位置对应。本实施例选用激光对射模块对伺服电机零位置进行确定，激光器接收模块在无光照的情况下输出高电平，当接受到电机转动台上的激光发射信号后变为低电平，所以对激光对射模块的安装精度有一定的要求。以下为零位自检程序while(ZERO)
{
Uartl_PutString(zero,9);//向伺月艮电机发送自转命令
if(ZERO==0)//如果转动到O位置 {
UartI_PutString(reset,9); //向伺服电机发送复位命令 delay _ms( 1000);
if(ZERO==0)//判断确实为O位置{
Uart I PutString(reset,9);
}
}
}·ARM控制器在串口中断函数中开始接收PC机发送过来的数据，为了记录串口的工作状态和接收数据的个数，本文自定义了一个通信协议——首先定义了一个64字节的接收缓冲数组USART_RX_BUF [64]和接收状态标识USART_RX_STA，其中第7位为接受完成标志位,第0-5位为接受到的数据个数，其中每次传递的数据包以特殊字符“？ ”为结束标志，每条指令以字符“；”为结束符。上位机ARM每次接受到的数据最大缓冲为64个字节，而无线数据传输模块每次最多只能发送32个字节的数据包，所以在发送数据之前要把64字节的数据USART_RX_BUF[64]分割为4个16位的数据TX_BUF(T3 [16]，这样能保证无线发射的数据不会丢失。每次发射16位数据后要有一定的延迟时间delay_ms(3)，此延迟的时间不能太长，否则会在还没有发送下面数据的时候就被新数据覆盖，延迟时间也不能太短，因为下位机发送指令到伺服电机执行要有一定的执行时间。经过反复测试每次发送完数据后延迟时间3ms为佳。最后将接收到的字节发回串口进行回显，并且每次发送一个数据包LED灯闪烁一下，以此来指示操作是否成功以及显示无线发送的状态。上位机的ARM控制器和下位机的ARM控制器的主程序主要包括Cortex_M3系统初始化、零位置自检、无线模块和陀螺仪模块初始化，以及串口中断处理函数，其中为了降低串口数据传输的时间，将串口通信的波特率设为115200的程序流程图如图3、图4所示。监控软件程序主要包含两个部分视频显示部分和串口通信部分。第一部分采用ezdivcap控件,该控件是一个基于Microsoft的DirectShow技术开发的音视频信号捕捉ActiveX控件。它基于DirectShow过滤器组件技术设计,采用完整的ActiveX封装,可用于多种软件开发平台，.全面支持1394、USB热拔插数字视频捕捉设备。第二部分采用MSComm串口通信控件，监控程序运行界面如图5所示。发明人根据本实施例进行了如下测试，测试步骤如下一、安装硬件以及正确接线，将带有陀螺仪的上位机ARM控制器安装到被跟随的物体上，安装激光对管，将接收器安装到标定的零位置；二、安装上位机监控软件，给伺服电机和驱动器上电；三、打开视频信号获取通道和串口号，在控制窗口输入电机的运动参数，如运动控制模式、PID、线数、最大跟踪误差和倍频等；四、先后给上位机和下位机ARM控制器上电，伺服电机会自行运动到激光对射模块标定的零位置；五、转动被跟随物体到一定位置，读取伺服电机的转速和转过的角位置并记录，将上位机系统放到中间有一堵实体墙间隔的位置，重复以上步骤，每次测试重复做5次，取均值记录数据如图6所示。实验性能参数如下1、无线传输无线传输距离和抗干扰能力主要与无线模块的功率以及发射接收之间是否有障碍物有关。在温度为28°、湿度为30%和无线数据模块的传输速率为2M的情况下，实测空旷地带稳定传输距离为1000米左右，有障碍物——一堵实体墙的情况下能够稳定传输的距离为50米左右。2、随动响应范围 响应范围是指伺服电机响应的最大和最小角速度的范围,本参数主要与陀螺仪的测量范围和伺服电机驱动器有关。在系统中驱动器响应范围在IOOOrpm之上，换算成dps之后为6000dps,所以实施例的响应范围主要和陀螺仪的测量范围有关，即 ±250dps、±500dps、±2000dps。3、随动响应延迟响应延迟是指在获取陀螺仪角速度之后到伺服电机开始转动之间的时间差，本参数主要与电路系统电气反应延迟Te和伺服电机机械响应延迟Tm有关。电气反应延迟时间Te主要和陀螺仪数据输出频率Tu、无线模块发送和接受数据包的时间TESD、数据包之间的数据间隔TD、串口发送时间Ts和最大加速时间Ta有关；伺服电机机械响应延迟Tm主要与驱动器的S曲线加速和电机的励磁有关。其中电气响应时间1=1^+1^+1+1^+1^=7.65ms，机构响应时间TM=7ms,最大响应时间Tmax=TE+TM=15mSo4、随动响应误差响应误差主要是指伺服电机做跟随运动转过的角度和运动主体实际转过角度的差值，总响应误差分为原理误差、系统误差、操作误差和环境误差。实施例中原理误差主要是因为对角速度的采集频率不能达到无穷大，数据传输的时间不可能达到无穷小。系统误差主要是由硬件性能引起的，包括陀螺仪的误差、伺服电机和驱动器产生的误差。操作误差主要是由元器件和激光对管等的安装以及人员操作等因素有关。环境误差主要是由于测试平台的震动有关。鉴于以上误差源。本实施例采取以下相应措施将陀螺仪的数据输出频率设置为800HZ，即每1. 25ms采集一次数据；开启无线模块中断，设置串口的波特率为115200bps ;将时钟信号9倍频之后，ARM控制器的处理速度为72MHZ ;开启陀螺仪高低通滤波器并对采集到的角速度值进行软件滤波处理；设置伺服电机驱动器的最大加速度值为65535，这样可以将伺服电机S曲线变换的时间降到最低。图7降低误差前采集到的角速度，图8为经过以上措施降低误差处理后采集到的角速度。实验证明，本实施例在空旷地带能稳定传输1000米，一堵实体墙能够稳定传输50米，最大随动响应范围±2000(1 8，最小运动控制精度0.09°，最大响应时间Tmax为15ms，系统响应误差I ε匕为O. 174°。实施例作用与效果综上所述，本实施例中的角位置无线随动装置及其方法能够实现远距离无线随动控制和指令控制两种控制方式，并且可以远程无线视频观察伺服电机的运转情况。且无线传输距离远，数据传输稳定可靠，伺服电机对陀螺仪的跟随性能(实时性)好，运动响应快，运动控制精度高，误差小 ，抗干扰能力强。保障了在多个工作领域下的稳定性和实用性。
权利要求
1.一种角位置无线随动控制装置及其方法，其特征在于，包括安装在被随动物体上或者监控室的上位机模块；以及，安装于应用现场，具有伺服电机的下位机模块；其中，所述上位机模块具有数据采集部，采集所述被跟随物体的转动角速度，并将产生相对应的角速度信号； 上位机控制部，将所述角速度信号进行处理，转换成相对应的控制信号；上位机通信部，将所述控制信号送至所述下位机模块；所述下位机模块具有下位机通信部，接收所述控制信号；控制驱动部，对所述伺服电机进行零位置校准，在完成所述零位置校准之后，根据所述控制信号驱动所述伺服电机，完成角位置同步运动。
2.根据权利要求1所述的角位置无线随动控制装置，其特征还在于其中，所述上位机模块还具有显示部，下位机还具有视频采集部；所述视频采集部，通过摄像头，采集所述伺服电机的运动状况，并转换成相应的视频信号;所述下位机通信部还将所述视频信号送出至上位机通信部；所述显示部，根据所述上位机通信部接收到的所述视频信息，显示所述运动状况。
3.根据权利要求1所述的角位置无线随动控制装置，其特征还在于其中，上位机控制部还具有指令输入部，供用户输入控制指令，并将所述控制指令转换成相应的指令信号，传输给所述下位机模块，控制所述伺服电机运动。
4.根据权利要求1所述的角位置无线随动控制装置，其特征还在于其中，所述控制驱动部通过激光对射模块对所述伺服电机进行所述零位置校准，所述激光对射模块先检测所述伺服电机是否停留在零点位置，即所述激光对射模块的安装位置；若不在，则驱动所述伺服电机至所述零点位置，并发出复位命令。
5.根据权利要求1所述的角位置无线随动控制装置的角位置控制方法，其特征在于， 包括如下步骤(1)所述数据采集部实时获取所述被测物体的所述转动角速度；(2)所述上位机控制部对所述转动角速度进行软件滤波、数据打包、数据分析后形成所述控制信号；(3)所述上位机通信部将所述控制信号传输至所述下位机模块；(4)所述下位机控制部先将所述伺服电机进行所述零位置校准，然后根据所述控制信号，驱动所述伺服电机进行角位置同步运动。(5)所述视频采集部将所述伺服电机的所述运动状况发送至所述显示部，供用户实时观察所述伺服电机的运动状况。
6.根据权利要求5所述的角位置无线随动控制装置的角位置控制方法，其特征还在于其中，用户可跳过步骤(I)、步骤(2)，直接在所述控制部直接输入指令，在所述控制部将所述指令转换成指令控制信号后继续步骤(3广步骤(5)，完成指令控制所述伺服电机进行所述角位置同步运动。
全文摘要
本发明提供的一种角位置无线随动控制装置以及方法，其特征在于，包括安装在被随动物体上或者监控室的上位机模块；以及，安装于应用现场，具有伺服电机的下位机模块；通过无线传输数据，达到实现无线随动控制的效果。本发明的角位置无线随动装置及其方法能够实现远距离无线随动控制和指令控制两种控制方式，并且可以远程无线视频观察伺服电机的运转情况。且无线传输距离远，数据传输稳定可靠，伺服电机对陀螺仪的跟随性能(实时性)好，运动响应快，运动控制精度高，误差小，抗干扰能力强。保障了在多个工作领域下的稳定性和实用性。
文档编号G05D3/12GK103034246SQ201210510098
公开日2013年4月10日 申请日期2012年12月3日 优先权日2012年12月3日
发明者任强, 孙玉国, 周升良, 房海先 申请人:上海理工大学