基于自适应变前向距离的球形机器人水面运动控制方法技术

本发明专利技术公开了一种基于自适应变前向距离的球形机器人水面运动控制方法，其包括以下步骤：校准航向角零位，启动脚本接收IMU数据；将航向角变为东北地坐标系；读取设置好的球形机器人的期望路径；根据当前状态测量值和期望路径，利用自适应变前向距离算法得到球形机器人的当前的航向角与期望的航向角的偏差；根据球形机器人的当前的航向角与期望的航向角的偏差，计算期望角速度；球形机器人的推力分配单元依照期望角速度分配左右螺旋桨的转速，使航向角和路径跟踪误差收敛，直至走完路径。本方案在设计架构上更加简单，且不依赖于模型因此对于各种不同的曲线方程和不同的船体都具有良好的鲁棒性，在逼近期望路径时能有非常平滑的逼近效果。的逼近效果。的逼近效果。

【技术实现步骤摘要】
基于自适应变前向距离的球形机器人水面运动控制方法


[0001]本专利技术涉及球形机器人控制领域，尤其是涉及一种基于自适应变前向距离的球形机器人水面运动控制方法。

技术介绍

[0002]水陆两栖球形机器人采用的球形结构相较于传统的舰船结构，在水中其转向超调大、控制航向角难度增加，在期望路径存在较大转折时，容易出现超调现象，造成弯折处的路径跟踪误差变大。常规的制导律对于曲线路径跟踪的制导效果较差。

技术实现思路

[0003]本专利技术主要是解决现有技术所存在的对于曲线路径跟踪的制导效果较差的技术问题，提供一种对各种曲线形式都具有较好的鲁棒性、可以让球形机器人在逼近期望路径时能有非常平滑的逼近效果的基于自适应变前向距离的球形机器人水面运动控制方法。
[0004]本专利技术针对上述技术问题主要是通过下述技术方案得以解决的：一种基于自适应变前向距离的球形机器人水面运动控制方法，包括以下步骤：
[0005]S1、校准航向角零位，启动脚本接收IMU(Inertial Measurement Unit，惯性测量单元)数据；本方法基于的航向角是从正北方向开始算起，根据东北地坐标和右手系法则，顺时针为航向角正方向，但是IMU测得的初始航向零位为启动IMU数据接收脚本的瞬间的球的朝向，所以需要校准航向角零位后，再启动相应脚本；
[0006]S2、对IMU反馈的航向角做坐标变换，变为东北地坐标系；IMU航向角基于东北天坐标系，所以需要进行坐标变换；
[0007]S3、读取设置好的球形机器人的期望路径；期望路径用事先给定的两个参数式方程x(t)和y(t)表示，为曲线路径；
[0008]S4、接收来自IMU和启动RTK(Real Time Kinematic，实时动态测量技术)后的GPS数据作为状态测量值，根据当前状态测量值和期望路径，利用自适应变前向距离算法得到球形机器人的当前的航向角与期望的航向角的偏差；
[0009]S5、根据球形机器人的当前的航向角与期望的航向角的偏差，计算期望角速度；
[0010]S6、球形机器人的推力分配单元依照期望角速度分配左右螺旋桨的转速，使航向角和路径跟踪误差收敛；
[0011]S7、判断球形机器人是否走完跟踪路径，如果已经走完，航向控制结束；如果没有走完跟踪路径，重复执行步骤S2到步骤S6，直至走完路径。
[0012]作为优选，所述步骤S4中，利用自适应变前向距离算法得到球形机器人的当前航向角与期望的航向角的偏差的步骤具体为：
[0013]S401、获得球形机器人与期望路径的最小距离、最小距离点以及t1；最小距离是指球形机器人与期望路径的最小距离，最小距离点为期望路径上距离球形机器人最近的点(x(t1),y(t1))，最小距离是指最小距离点和球形机器人之间的距离，t1是是指最小距离点用
两个参数式方程表现时对应的参数t值；
[0014]S402、计算当前航向角和期望路径在t1位置处的切向量夹角e
psi
；
[0015]S403、当最小距离大于当前航速10秒内的移动距离时，前向距离设置为0；
[0016]当最小距离小于10倍航速数值之间的距离时，通过以下公式确定前向距离val：
[0017][0018]式中，A、B和C为经验参数；典型值A＝7,B＝0.1,C＝40；
[0019]S404、将前向距离转化为参数t的变化量dt值，dt所对应的弧长为d，计算弧长d对应的参数t2的值；即t2＝t1+dt，这里的参数均指两个参数式方程x(t)和y(t)的参数；
[0020]S405、计算当前球形机器人所处位置到前向距离点所连向量与y轴夹角，即为期望航向角，通过相减获得当前航向角与期望的航向角的偏差。
[0021]即在水面球形机器人距离曲线较近时，如果e
psi
较大，说明球形机器人更像是以倾向于90度(甚至更大)的角度冲向曲线，则此时应该把前向距离调大，让球形机器人以更加平滑的角度逐渐趋近期望路径。如果e
psi
较小，说明球形机器人的朝向已经与路径的切线方向比较吻合，此时可以适当减小前向距离。此时减小前向距离的好处有两个，其一是当球形机器人还未正式抵达期望路径时，可以以更高的逼近速度趋向期望路径；其二是当球形机器人已经抵达期望路径时，较小的前向距离能够让球形机器人更好地应对曲率较大的情况。因此，e
psi
和前向距离大体成正比关系。
[0022]作为优选，步骤S403中，当最小距离小于2倍航速数值之间的距离时，确定前向距离后，采用S面控制算法对前向距离进行修正：
[0023][0024]z为前向距离的偏移量，将z叠加到原有的前向距离上完成修正，k1和k2为参数，g代表最小距离与期望最小距离的偏差归一化处理后的值，代表g的变化率归一化处理后的值，由偏差对采样时间间隔作商求得。典型值是k1为4，k2为0.8，采样时间间隔为0.01秒。
[0025]在一些特殊情况下(比如曲线为阿基米德螺线等曲率随位置不断变动的复杂曲线)，在水面球形机器人抵达期望路径后，会出现围绕期望路径做正弦波状振荡的现象，平均横向偏差通常不超过1米，但是显然显著降低了航向效率。原因在于曲率变化较快，球形机器人的前向距离过小导致仍然存在不可忽视的过冲现象。因此，有必要对最小距离min_dist很小的情况进一步优化前向距离。故采用S面控制算法来对dt做额外偏移量。
[0026]前向距离定义为从最小距离点开始，沿着曲线参数t增加的方向增长的弧长长度。前向距离的选取非常关键也存在难度。一方面，如果前向距离过近，有很大可能会使球形机器人在接近期望路径时出现过冲；另一方面，如果前向距离过大，很容易会让球形机器人的收敛到期望路径的速度过慢。而本方案既可以避免出现过冲，也可以具有较好的收敛速度。
[0027]作为优选，步骤S401中，最小距离由以下方式求得：
[0028]在一个指定的有限区间内，首先尝试使用抛物线插值法寻求最小值，如果失败，则使用黄金分割法寻求最小距离。
[0029]实际应用时，求最小距离的过程封装为一个代码函数，有限区间在调用代码函数
时指定；通过调用代码函数，输入指定曲线参数t区间和待求最小值的函数，直接获得最小距离、最小距离点以及对应的参数值t1。
[0030]距离函数为：
[0031][0032](x1,y1)为球形机器人坐标。
[0033]作为优选，步骤S5中，期望角速度通过PID控制的方式得到。
[0034]在使用PID控制让机器人的实际角速度跟踪期望角速度时，控制器将计算期望角速度和实际角速度之间的差距，并根据这个差值来调整控制输入。PID控制器由三个部分组成：比例(P)，积分(I)和微分(D)。比例(P)部分是误差的简单倍数。比例增益(Kp)决定了响应的强度。如果误差很大，比例项将增加控制输入，以更快地减少误差。但是，仅使用比例控制通常会导致超调。积分(I)部分是误本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于自适应变前向距离的球形机器人水面运动控制方法，其特征在于，包括以下步骤：S1、校准航向角零位，启动脚本接收IMU数据；S2、对IMU反馈的航向角做坐标变换，变为东北地坐标系；S3、读取设置好的球形机器人的期望路径；期望路径用两个参数式方程x(t)和y(t)表示；S4、接收来自IMU和启动RTK后的GPS数据作为状态测量值，根据当前状态测量值和期望路径，利用自适应变前向距离算法得到球形机器人的当前的航向角与期望的航向角的偏差；S5、根据球形机器人的当前的航向角与期望的航向角的偏差，计算期望角速度；S6、球形机器人的推力分配单元依照期望角速度分配左右螺旋桨的转速，使航向角和路径跟踪误差收敛；S7、判断球形机器人是否走完跟踪路径，如果已经走完，航向控制结束；如果没有走完跟踪路径，重复执行步骤S2到步骤S6，直至走完路径。2.根据权利要求1所述的基于自适应变前向距离的球形机器人水面运动控制方法，其特征在于，所述步骤S4中，利用自适应变前向距离算法得到球形机器人的当前航向角与期望的航向角的偏差的步骤具体为：S401、获得球形机器人与期望路径的最小距离、最小距离点以及t1；最小距离是指球形机器人与期望路径的最小距离，最小距离点为期望路径上距离球形机器人最近的点(x(t1),y(t1))，最小距离是指最小距离点和球形机器人之间的距离，t1是是指最小距离点用两个参数式方程表现时对应的参数t值；S402、计算当前航向角和期望路径在t1位置...

【专利技术属性】
技术研发人员：王酉，曹勖之，邹虹群，
申请(专利权)人：逻腾杭州科技有限公司，
类型：发明
国别省市：