一种巡视器闭环跟踪控制方法
【专利摘要】一种巡视器闭环跟踪控制方法，巡视器接收并解析地面发送的指令，根据指令内容进入位置闭环控制或者位置及航向闭环控制或者路径跟踪控制流程；本发明通过一次性给定闭环控制目标点并定周期执行位置控制，巡视器即可实现持续朝向目标点运动；通过一次性给定闭环控制目标点及期望偏航姿态并定周期执行位置与航向闭环控制，巡视器即可实现持续朝向目标点运动，且最终以期望的偏航方向接近目标点；通过定周期执行闭环跟踪目标点选取和位置与航向闭环控制，巡视器即可实现持续跟踪期望的规划路径。
【专利说明】一种巡视器闭环跟踪控制方法
【技术领域】
[0001]本发明提出了一种闭环跟踪控制方法，对路径跟踪控制、点对点控制、方位角受控的点对点控制进行了整合，使用同一组控制方程，通过对控制输入项及控制参数进行调节，可以分别实现位置闭环控制、位置及航向闭环控制、路径跟踪控制等不同的控制目标。本方法可以推广应用于一般遥控式漫游车辆的闭环跟踪控制。
【背景技术】
[0002]巡视器具有四轮转向、六轮驱动的摇臂悬架式底盘结构，可以被动适应自然起伏地形，因此属于具有非完整约束的轮式移动机器人。根据控制目标的不同，非完整系统的反馈控制问题大致可分为三类:点镇定、路径跟踪、轨迹跟踪。
[0003]点镇定即控制机器人到达运动平面上的任意给定的某个目标点，并且能够稳定在该目标点；路径跟踪，即控制机器人能够到达并最终以给定的速度跟踪运动平面上给定的某条路径；轨迹跟踪，即控制机器人能够到达并最终以给定的速度跟踪运动平面上给定的某条轨迹。简单来说，后两者的区别是路径跟踪只要求机器人能够跟踪已经规划好的路径，对何时到达何点不作要求，而轨迹跟踪要求机器人实时跟踪按时间变化的参考轨迹。
[0004]对于巡视器而言，由于滑移的影响，巡视器的速度并不是精确可控的，而且巡视器的任务需求也并不要求实现轨迹跟踪，因此，仅需考虑巡视器的点镇定控制和路径跟踪控制即可。一般认为，对点镇定问题和路径跟踪问题，需要设计不同的控制器来予以解决，因为点镇定属于镇定控制，路径跟踪属于跟踪控制，是不能通过一种控制律来解决的。但在工程实践中，这样会增加控制算法 设计的复杂性，占用系统资源，不利于提高移动机器人系统的效率。事实上，只需经过必要的整合，是可以采用统一的控制律来同时满足巡视器目标点控制和路径跟踪控制的需求的。
【发明内容】
[0005]本发明的技术解决问题是:克服现有技术的不足，提供一种轮式机器人(例如巡视器)的闭环跟踪控制方法。
[0006]本发明的技术解决方案是:一种巡视器闭环跟踪控制方法，步骤如下:巡视器接收并解析地面发送的指令，根据指令内容进入位置闭环控制或者位置及航向闭环控制或者路径跟踪控制流程；其中，位置及航向闭环控制步骤如下:
[0007](I)从解析的指令内容中获取闭环控制目标点的位置及偏航方向(xt，yt，￥t)；
[0008](2)根据(xt，yt，￥t)以及巡视器当前位置及航向(xe，y。，Ψ。)，计算巡视器当前位置与期望位置的水平投影距离et、航向偏差α,以及航向角修正项Y。距离修正项&;将et, Yt作为闭环控制中的状态向量，其中Y t= a t-y ;
[0009](3)判断et是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤(4);
[0010](4)根据步骤(2)中确定的状态向量et，Y t，计算巡视器当前控制周期的期望线速度Vrai和期望偏航角速度ωez;控制巡视器按照计算的期望线速度Vrai和期望偏航角速度运动；进入下一控制周期时转步骤(2)循环执行；
[0011]位置闭环控制步骤如下:
[0012](1)从解析的指令内容中获取闭环控制目标点的位置(xt，yt)；
[0013](2)根据(xt，yt)以及巡视器当前位置及航向(U。，Ψ。)，计算巡视器当前位置与期望位置的水平投影距离et、航向偏差atjfet，Y t作为闭环控制中的状态向量，其中Y t= α t ；
[0014](3)判断et是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤(4);
[0015](4)根据步骤(2)中确定的状态向量et，Y t，计算巡视器当前控制周期的期望线速度Vrai和期望偏航角速度ωεζ ;控制巡视器按照计算的期望线速度Vrai和期望偏航角速度运动；进入下一控制周期时转步骤(2)循环执行；
[0016]路径跟踪控制步骤如下:
[0017](I)从解析的指令内容中获取期望的运动曲L爻期望的运动里程Iep ;
[0018](2)判断巡视器从起始时刻至当前位置的运动总里程I。是否大于或等于Iep，若是，则本次控制结束；否则根据上述运动曲率确定当前控制周期的跟踪目标点作为当前闭环控制目标点，其位置及偏航方向记为(xt，yt，vt)；
[0019](3)根据(xt，yt，￥t)以及巡视器当前位置及航向(Xc，yc，Ψ。)，计算巡视器当前位置与期望位置的水平投影距离et、航向偏差α,以及航向角修正项Y。距离修正项&;将et, Yt作为闭环控制中的状态向量，其中Y t= a t-y ;
[0020](4)根据步骤(3)中确定的状态向量et，Y t，计算巡视器当前控制周期的期望线速度Vrai和期望偏航角速度ωεζ ;控制巡视器按照计算的期望线速度Vrai和期望偏航角速度运动；进入下一控制周期时转步骤(2)循环执行。
[0021]所述的航向角修正项Y1?计算公式如下:
[0022][0023]
【权利要求】
1.一种巡视器闭环跟踪控制方法，其特征在于步骤如下:巡视器接收并解析地面发送的指令，根据指令内容进入位置闭环控制或者位置及航向闭环控制或者路径跟踪控制流程；其中，位置及航向闭环控制步骤如下: (1)从解析的指令内容中获取闭环控制目标点的位置及偏航方向(xt，yt，vt)； (2)根据(xt，yt，￥t)以及巡视器当前位置及航向(Xc;，y。，Ψ。)，计算巡视器当前位置与期望位置的水平投影距离et、航向偏差at以及航向角修正项L、距离修正项4;将％，Yt作为闭环控制中的状态向量，其中gamma t= a t-gammar-er ; (3)判断et是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤(4)； (4)根据步骤(2)中确定的状态向量et，Yt，计算巡视器当前控制周期的期望线速度和期望偏航角速度ωεζ;控制巡视器按照计算的期望线速度Vra￡和期望偏航角速度运动；进入下一控制周期时转步骤(2)循环执行； 位置闭环控制步骤如下: (1)从解析的指令内容中获取闭环控制目标点的位置(xt，yt); (2)根据(xt，yt)以及巡视器当前位置及航向hΨ。)，计算巡视器当前位置与期望位置的水平投影距离et、航向偏差at;将et，Y t作为闭环控制中的状态向量，其中Y t= α t ； (3)判断et是否小于目标点判定阈值，若小于，则认为已到达目标点，本次控制结束；否则进入步骤(4)； (4)根据步骤(2)中确定的状态向量et，Yt，计算巡视器当前控制周期的期望线速度和期望偏航角速度ωεζ;控制巡视器按照计算的期望线速度Vra￡和期望偏航角速度运动；进入下一控制周期时转步骤(2)循环执行； 路径跟踪控制步骤如下: (1)从解析的指令内容中获取期望的运动曲2〔反期望的运动里程 (2)判断巡视器从起始时刻至当前位置的运动总里程I。是否大于或等于Iep，若是，则本次控制结束；否则根据上述运动曲率确定当前控制周期的跟踪目标点作为当前闭环控制目标点，其位置及偏航方向记为(xt, yt, vt)； (3)根据(xt，yt，￥t)以及巡视器当前位置及航向(Xc;，y。，Ψ。)，计算巡视器当前位置与期望位置的水平投影距离et、航向偏差at以及航向角修正项L、距离修正项4;将％，Yt作为闭环控制中的状态向量，其中gamma t= a t-gammar-er ; (4)根据步骤(3)中确定的状态向量et，Yt，计算巡视器当前控制周期的期望线速度和期望偏航角速度ωεζ;控制巡视器按照计算的期望线速度Vra￡和期望偏航角速度运动；进入下一控制周期时转步骤(2)循环执行。
2.根据权利要求1所述的一种巡视器闭环跟踪控制方法，其特征在于:所述的航向角修正项Y1?计算公式如下:
3.根据权利要求1所述的一种巡视器闭环跟踪控制方法，其特征在于:所述的距离修正项的计算公式如下:
4.根据权利要求1所述的一种巡视器闭环跟踪控制方法，其特征在于:所述的期望偏航角速度计算公式如下:
【文档编号】G05D1/10GK103760908SQ201410003688
【公开日】2014年4月30日 申请日期:2014年1月3日 优先权日:2014年1月3日
【发明者】邢琰, 滕宝毅, 刘祥, 毛晓艳, 贾永, 王大轶, 何英姿, 陈建新, 何健, 刘云, 张晋, 万丽景, 党纪红 申请人:北京控制工程研究所