全场定位-设计修改版

ThedesignedofControlSystemforthrowingFrisbeeSincetheinventionofthefirstrobot,therobotresearchhasbeenahottopicofscientificresearch.Withthedevelopmentofinligentmanufacturing,therobotismoreandmorewidely.TheresearchsubjectofthispaperistodesignacontrolsystemofthrowingFrisbeerobotwithomnidirectionalchassis,thegoalistoachievethe"sixteenthrobotcontest"thetaskofcompetition.Followingisthemaincontentofthisstudy.Accordingtotherulesofthegame,toachieveautonomousrobotpositioning,throwingFrisbeefrisbeeanddetectionfunction.Compliancewiththedesignprinciplesofmodulardesignofthecontrolsystem,maincontrolmodule,positioningsystem,emissioncontrolsystem,detectionsystemandcontrolsystem.ThemaincontrolmodulebyARMSTM32F407processorequippedwithUCOSIIIoperatingsystem.Therealizationofeachmoduleschedulingthroughthetaskschedulingmechanism.Amongthem,thepositioningsystemasoneofthemostimportanttechnologyintherobot,itisamajorfeatureofthisresearch.Inthepositioningsystem,basedonthelocationofthequadratureencoder-gyroscopemodelafterthecompletionofdataacquisition,whichcanresolvetheposeoftherobot.Thencompletethespeedoutputthroughthedesignwithspeedcurvethelawofsinecurve.DuringtheuseofPIDcontrolstrategytoimprovetheprocessofclosedloopcontroltoensureaccuracy.Finally,usinglaserradarsensorinformationreparsedmachinepositiontocompletethetwostepcalibration,ensurepositioningaccuracy.Finaly,arobotwithahighlyaccuratepositioningsystemcanperformeffectivelanding,hitting,andshootingdowntheopponent'sfrisbeeandothertasks.Keyword:robot;localizationtechnology;PIDcontrol摘 第一章绪 前言 研究课题及其意 国内外发展状 课题研究内 第二章控制系统方案指标制 2.1机器赛简 控制系统方案指 第三章控制系统方案设 控制系统方案设 控制系统方案整体架 主控模块方案设 3.5防守模块方案设 手柄模块方案设 通信模块方案设 落盘功能方案设 控制系统电源模 第四章移动方案设 全方位移动控制系统方案简 机器位资数据获取方案设 位资获取系统的模型设 机器人速度更新模 机器人圆弧更新方 定位移动运动曲线的设 定位闭环方案设 定位闭环系统的方 移动的功能结 第五章理论分析与实 移动速度曲线控制效果测 定位移动闭环系统整体测 第六章总结与展 总 参考文 致 附录一：硬件电路 附图2：定位模块原理 附录二：程序代 第一章前言研究课题及其本课题背景为2017年“大学生机器赛”，竞赛的是“舞盘雅击打掉着上的球，并将飞盘落在着上计分【1。国内外CHING-CHANGWONG2005med2006年提出一种基于回归神经网RNN的全方位机器人速度控制方法。针对全方位机器人的速度控制问题。作者提出一种基于RNN的新型神经网络：ESN。ESN神经网络以机器人的速度为神经网络输入，三个电机的为输同时已经发展成各种PID控制策略也是黑箱模型的建模方式之一。并且课题研究内第二章系统方案指前言2.1机器赛简图2-1为比赛场地三维图（左）和赛场地说明图（右。场地内共有7根柱子， 图2-1比赛场地三维 比赛场地说明盘将球击落。最终，在规定的时间3分钟内得分最多，或提前完成任务（所有的着控制系统功能控制系统方案抛掷点指标定9m落盘需要的角度精度为：2.36m击球需要的角度精度为：1.33m击盘需要的角度精度为：1.112m，平移误差（XY相等500mm；落盘平移精度要求2m，平移误差（XY相等：375mm；击球平移精度要求：行程2m，平移误差（XY相等：70mm；机器人在移动2m（相邻柱子间）距离时，角度精度需求应为：1.1457度。平移精度为：60mm。同时机器人需要在3分钟内完成比赛，因此需要移1.7m/s第三章统方前言控制系统方案图3-1控制系统总体结构图主控模块方案成嵌入式实时操作系统UCOSIII作为软件设计平台借助它的任务管理系如图3-2。图3-2主控软件框图接收的信MiniPC解析信号并执MiniPC信号并执图3-3机器人系统任务表主控模块采用的是ARM处理器STM32F407ZGT6作为控制了方便调试加入了LCD显示功能、调试状态LED灯、蜂鸣器和调试按键等。主控模块与其他子系统的通讯方式为USART，此处使用MAX3232将ARM输出的TTL电平和232电平进行相互转换【6。USART部分的电路原理图如图3-4。图3-4USART飞盘发射执行系统方3-使用LCD显示任务和按键解析任务构成一个大循环结USART中断任务飞盘发射控制系统采用的也是ARM处理器STM32F407ZGT6作为控制单元，同时也加入了LCD显示功能、调试状态LED灯、蜂鸣器和调试按键等调试模块。因为发射模块驱动元件较多，需要合理的分配硬件资源。资源分配图如图3-6。图3-6飞盘发射系统的软件框架其中电磁阀的驱动电路使用的是基于MOS的放大电路进行驱动。电路原理图如图3-7：图3-7如图3-8。图3-8稳压电路原理图防守模块方案防守模块主要负责提供对飞盘的检测，给击盘功能提供对手飞盘在3m着觉传感器完成。视觉传感器使用的是USB头。软件平台为Windos10操作系统，利用成计算机视觉裤OpenCV进行算法开发。在头完成飞盘检测后，便可得到飞盘在头视野中的位置手柄模块方案通信模块方案②全方位平面的通信方式：USART串口发射模块：①电机控制器的通信方式：CAN总线防守模块：①头通信方式：USB连接手柄模块：①USARTUSART串口。落盘功能方案执行流程：操作手发送落盘的柱子序号→主控调控到达指定位置，图3-9落盘功能结构图防守击盘功能击盘功能结构如图3-10，击盘功能除了结合外，还需要借助激光→主控调控进行相应的偏移，以求对准对方飞盘→待定位结束后，执行图3-10控制系统电源模块。电路原理图如图3-11。图3-11稳压电路原理图第四章移动方案设前言本章内容：移动的设计作为本课题研究的特色部分，特此章节对其全方位移动控制系统图4-1机器位资数据获取方装方式较简单，且安装难度低。编安装方式如图4-2：BB

图4-2正交 安装形YY编AX机器人坐标系OX'Y中定义的机器人速度(vxvy由编正交编测量。如图4-2，编A、B在严格与OX'、OY'轴平行的前提下，将机器人底盘视作vxvAvyvB其中，vx、vy是机器人底盘速度，vA、vB是编轮线速度，kA、kB是编码器系数，kA、kB的确定需要依据编轮直径和编线数；nA、nB是编我们可将一定中断时间段tvxvy,的机器人运动。故每一小段时间内，机器人均做圆弧运动（各t对应圆一：机器人绕地面坐标系原点O旋转参数使机器人行走，旋转角度，经t时间段机器人到达(x,y点。要得到机器人位置参量(x,y)，我们首先将机器人视为质点，假设机器人从当前x0,y0,0)状态下绕地面坐标系原点O旋转角度：质点绕原点ORot(O,)sin

sin 0 0 0x0ysin0y0 1 0x0ysin0y0 10 上式即为绕地面坐标系原点O旋转角度，质点的坐标更新。其中，x0y0为上一时刻机器人的全场坐标，xy为当前时刻机器人的全场坐标；为机器人绕着原点O旋转的角度。实际情况下，机器人在t时间段的行走是绕着任意一点A(k1,k2)做的旋转，如A(k1k2旋转转化成机器人绕地面坐标系原点O旋转的情况，做出分析，得到坐标更新。4-3，可利用两个平移操作得出质点B1任一A(k1,k2)旋转的坐标更图4-3将图中A、B1点平移，使A(k1,k2)与原点重合，B1点移至B2点。平移 Trans(A) ---20 1 Rot(O,)sin

sin 0 0 Trans(A) 0则可得到质点坐标更新

---1

xB

y c1Trans(A)Rot(O,)Trans(A)yB1 k1sink2cosk sin k1cosk2sink1x00ysin k1sink2cosk2y0 k1x0v1sin0v2cos k2y0v1cos0v2sin ，t人的全场坐标及姿态；为机器人绕着原点O旋转的角度。 其中，是机器人转动角

xvy(cos(t)cos)vx(sin(t)sinyvy(sin(t)sin)vx(cos(t)cos 期；(x,y,)是机器人当前位姿参量。机器人直线更新是在时间段t内将机器人运动轨迹视作直线的一种坐标更法的前提是0(0为角速度阈值)。基于平面运动机器人做分析，它的姿态角pitch、roll均不改变，这里利用直线更新方式。更新为：y(vysinvcosy(vysinvcosx定位移动运动曲线的设 的22的周期，和余弦函数的到图4-4正弦函 余弦函的变化规律。如图4-5。图4-5与时间t设定移动速度为VV1(t) 设计曲线是取正弦曲线的22段，为了方便算法设计和析将其向

y(t)sin(t) 平移后的曲线在0到2ymax的正弦规律，则速度曲线到

F1ty

V2(t)V1(t)(sin(t)y 其中正弦曲线在0到2ymaxy(2。2(t)V2(t)1(t)(cos(t)1tV

y

G(t)1(t)y

运用过程：当定位移动距离为V1(t)V则可得到速度输入曲 定位闭环方案设

学模型的确定性控制系统。因此考虑使用比较成xx+微积比图4-6PID检，恒电流制动（或称刹车）和PID调节技术的铭朗控制器。闭环系统的反馈数据机器人移动分为平移和自转，因此闭环分为平移的闭环和自转的闭环，移速度—位移闭环和旋转速度—角度闭环两部闭环算法基础为标准PID控制策4-7图4-7闭环框架图设定位移X1(t)为系统的输系统的反馈输入为X2(t)比较得到位移e(t)X1(t)X位移误差e(t)作为输入，经过PID控制器，即可得到速kVoffset(t)KP*e(t)KI*e(j)KD*[e(k)e(kj Vout(t)Vin(t)

等的影响，还会受到平移速度的影响，如图4-8所示，速度的变化会引起角度闭环图4-8角度调节分析图角度的影响。而采用普通的PID策略，已不能达到较好的调节效果。因此需要提出提高了参数的适应性和调节的灵活性，但是在目标发生变化时，增量式PID适合需要针对性设计其调节的闭环算法【12文提出了改进的位置式PID控制策略。如图4-9：图4- 改进的PID控制策此控制策略在位置式PID策略上，增加了辅助PID，辅助PID系统输入为实际纠偏量和用户配置的调节速度（调节周期）决定，辅助PID系统的输出将会对位置式PID的整定系数进行调节。此控制策略既达到自适应速度的目的，也保留位置式PID的特性。out(t)in(t) koffset(t)KP(t)*e1(t)KI*e1(j)KD*[e1(k)e1(kj 其中in(t)为角度输入曲线，out(t)即为经过闭环后的角度输出曲线，2PIDPIDTPIDin(t)in(t)(e(t)e(t1))e(t e(t其中(e(te(t1因子为本周期实际纠偏量，

kout(t)kp*in(t)ki*in(t)kd*[in(t)in(tjKp(t)Kp

(4-(4-相邻两周期的角度传感器陀螺仪的数据作为系统的反馈输入，经过PID调控修图4-10闭环控制框定位闭环系统的方案误差方案分析虑到赛场信息的可变性，选用最稳定的柱子位置信息作为误差的参考。图4- 位资获取模误差方案软件设向的夹角1a，2cb2000mm（比赛场地信息。通过余弦定理可以获取aaAb2c2a2a Xx1cYy1c*sin②绕原点旋转角度的获

如图4-12，两个柱子所在直线的斜率，可以切实反映的坐标系与图4-12位 模kb(y2(x2 （4-21180移 的功能结

如图4-13为移动执行一次定位的结构图。首先，通过操作手下发图4-13定位功能结构图第五章论分析与实前言4、全方位移动误差系统测移动速度曲线控制效设定机器的定位距离为3000mm，设定机器运行的最大速度在1.0~2.2m/s范围据如图5-1：图5-1速度曲线定位效果图经过实验数分析可知，在1.0~2.2m/s为：0~0m，此实验数据未虑全场的检误差，经测量，在此验中，全场块的测误差波动范围为：18mm~22mm，因此此实验开环控制结果，即开环位相对误差动围为：18m4m,符合实验预测结果。同时实验数据也说明，随平移速度—位移闭环偏差，摩擦、控制延时等因素都会影响定位的精度。闭环系统的作用是对机器人11.0m/s~2图5-2实验组1图5-3实验组2图5-4定位平移实验数据图5m，1.0~2.5m/s误差波动范围为：1~8mm7m,1.0~2.41m/s图5-4定位平移实验数据图定位移动闭环系统整定位平移实验数据（Y方向）：定位距05定位距离设定为比赛需要的2m，定位平移实验数据（Y方向）：定位距05定位角度实验数据：定位距离1005定位角度实验数据：定位距离1005图5- 定位角度实验数据分析，角度的误差波动范围为：0~11.1457120mm。定位精度评的累积误差进行，则方案的精度需要达到比赛要求精度，的信数数据05图5- 数据：移动距-50-5010数据比较1.1457120mm。 第六章与展总PID参考文《ABUROBOCON2016国内选拔活动主体与规则》[S2016亚太机规则.宗光华.机器人的创意设计与实践[M].：航空航天大学第2版:：电子工业，2013.3《第十六届大学生机器赛ROBOCON图册》[S]2016亚太机器人比赛规则.2016.9.CM4指南,四轮全方位移动机器人的建模和最优控制[J]. ,,,海.控制理论与应用,四轮全方位移动机器人的建模和最优控制[J]. ,,,海.控制理论与应用, ,丁.一种全方位移动机器人的运动分析与控制实现,，，柴PID参数先进整定方法综述[J].自动化学报，2000年第26卷：347等.C程序设计（第三版）[M].：技术，甘于奉献精神使得南昌大学机器人队技术水平不断进步，更富有。附录一：硬件附图1附图2附录二：程序void int*Now_distance_x,int*Now_distance_y, {staticintinit_distance_x=0;staticintinit_distance_y=0;staticfloatangle_init=0;staticintpiece_count=0;staticfloatt_to_x=0;staticintcontrol_count=0;staticfloatVX_Offset=0;staticfloatVY_Offset=0;staticintcycle_flag=0;staticintspeed_vx=0;staticintspeed_vy=0;staticintdistance_x=0;staticintdistance_y=0;staticintdistance_x_offset=0;staticintdistance_y_offset=0;staticintdistance_x_last=0;staticintdistance_y_last=0;staticfloatangle_speed=0;staticfloatangle_model=0;staticfloatangle_speed_Mid=0;staticfloatangle_offset=0;staticfloatangle_dst_offset=0;staticuint8_tadjust_flag=0;staticintdirection_flag=1;staticintdis_y_offset=0;if(0=={init_distance_y=angle_init=control_count=Accel_time/TIM_Piece; init_distance_x)/(float)Accel_time*SECOND_SPEED);{ SECOND_SPEED/2;adjust_flag=} if(target_angle>180)target_angle-=360;angle_offset=target_angle-angle_init;{{angle_direction=1;{angle_speed_Mid=((float)((angle_offset)/360*2*3.1416f*1000)/Accel_time*SECOND_SPEED*ROBOCON_W);angle_dst_offset=}}else{angle_direction=-{angle_speed_Mid=((float)((angle_offset+360)/360*2*angle_dst_offset=angle_offset+}{ 1000)/Accel_time*SECOND_SPEED*ROBOCON_W);angle_dst_offset=angle_offset;}}}{angle_speed_Mid=((float)((angle_offset)/360*2*3.1416f* *SECOND_SPEED*}cycle_flag=}elseif(piece_count<control_count&&1=={v=B*(sin(x-3.1416/2)+speed_vx=(int)(VX_Offset*(sin(t_to_x-3.1416f/2)+1));angle_speed=(int)(angle_speed_Mid*(sin(t_to_x-3.1416f/2)+1));distance=f(x)=-1/A*cos(Ax-3.14/2)+x (int)(((float)(Distance_AccelX init_distance_x)/(2*3.1416f))*(-cos(t_to_x-3.1416f/2)+ angle_model=angle_init+(((float)(angle_dst_offset)/(2*3.1416f))*(-cos(t_to_x-3.1416f/2)+t_to_x)); elseif(angle_model<-180) {{

t_to_x=2*3.1416f*(float)TIM_Piece*(float)piece_countspeed_vy=(int)(VY_Offset*(sin(t_to_x-3.1416f/2)+1)); } (BASE_MIN_TIME/2))){ (BASE_MIN_TIME/2); }{t_to_x=2*3.1416f*(TIM_Piece*piece_count-Accel_time{{speed_vy=(int)(VY_Offset*(sin(t_to_x-3.1416f/2)+1)); -3.1416f/2)+ BASE_LONG)+dis_y_offset;}}speed_vy=(int)(VY_Offset*(sin(t_to_x-3.1416f/2)+1));distance_y=init_distance_y+(int)(((float)(Distance_AccelY- }distance_y_offset=distance_y-speed_vx+=(int)(Distance_PD_param*distance_x_offset +0*(distance_x_offset-distance_x_last));speed_vy+=(int)(Distance_PY_param *distance_y_offset +0*(distance_y_offset-distance_y_last));distance_x_last=distance_x_offset;distance_y_last=distance_y_offset;Control_Angle_Walk3((float*) )angle_model, )P_param, )D_param, )speed_vx, ) )Speed_Accel);}t_to_x=2*3.1416f*(BASE_MIN_TIME/2)/BASE_MIN_TIME;t_to_x=(Accel_time-BASE_MIN_TIME);t_to_x=3.1416f;init_distance_x=init_distance_y=angle_init=piece_count=t_to_x=control_count=VX_Offset=VY_Offset=cycle_flag=speed_vx=speed_vy=distance_x=distance_y=distance_x_offset=distance_y_offset=distance_x_last=distance_y_last=angle_speed=angle_model=angle_speed_Mid=angle_offset=angle_dst_offset=0;adjust_flag=0;direction_flag=+-