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

1.本发明涉及球形机器人控制领域，尤其是涉及一种基于自适应变前向距离的球形机器人水面运动控制方法。


背景技术：

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


技术实现要素：

3.本发明主要是解决现有技术所存在的对于曲线路径跟踪的制导效果较差的技术问题，提供一种对各种曲线形式都具有较好的鲁棒性、可以让球形机器人在逼近期望路径时能有非常平滑的逼近效果的基于自适应变前向距离的球形机器人水面运动控制方法。
4.本发明针对上述技术问题主要是通过下述技术方案得以解决的：一种基于自适应变前向距离的球形机器人水面运动控制方法，包括以下步骤：
5.s1、校准航向角零位，启动脚本接收imu(inertial measurement unit，惯性测量单元)数据；本方法基于的航向角是从正北方向开始算起，根据东北地坐标和右手系法则，顺时针为航向角正方向，但是imu测得的初始航向零位为启动imu数据接收脚本的瞬间的球的朝向，所以需要校准航向角零位后，再启动相应脚本；
6.s2、对imu反馈的航向角做坐标变换，变为东北地坐标系；imu航向角基于东北天坐标系，所以需要进行坐标变换；
7.s3、读取设置好的球形机器人的期望路径；期望路径用事先给定的两个参数式方程x(t)和y(t)表示，为曲线路径；
8.s4、接收来自imu和启动rtk(real time kinematic，实时动态测量技术)后的gps数据作为状态测量值，根据当前状态测量值和期望路径，利用自适应变前向距离算法得到球形机器人的当前的航向角与期望的航向角的偏差；
9.s5、根据球形机器人的当前的航向角与期望的航向角的偏差，计算期望角速度；
10.s6、球形机器人的推力分配单元依照期望角速度分配左右螺旋桨的转速，使航向角和路径跟踪误差收敛；
11.s7、判断球形机器人是否走完跟踪路径，如果已经走完，航向控制结束；如果没有走完跟踪路径，重复执行步骤s2到步骤s6，直至走完路径。
12.作为优选，所述步骤s4中，利用自适应变前向距离算法得到球形机器人的当前航向角与期望的航向角的偏差的步骤具体为：
13.s401、获得球形机器人与期望路径的最小距离、最小距离点以及t1；最小距离是指球形机器人与期望路径的最小距离，最小距离点为期望路径上距离球形机器人最近的点(x(t1),y(t1))，最小距离是指最小距离点和球形机器人之间的距离，t1是是指最小距离点用
两个参数式方程表现时对应的参数t值；
14.s402、计算当前航向角和期望路径在t1位置处的切向量夹角e
psi
；
15.s403、当最小距离大于当前航速10秒内的移动距离时，前向距离设置为0；
16.当最小距离小于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)部分是误差随时间的累积和。在角速度控制时，积分增益(ki)帮助消除由于环境扰动因素引起的稳态误差。当机器人的角速度接近目标但由于扰动(可能是持续性扰动)而不能达到期望值时，积分项可以提供额外的控制输入。微分(d):微分部分是误差的变化率。微分增益(kd)有助于预测系统的行为，并减少超调和振荡。当实际角速度快速接近其目标角速度时，微分项可以减少控制输入，使其更平滑地接近目标。
[0035]
此外，还可以采用lqr、滑膜控制、模糊控制、s面控制等方式来获得期望角速度。
[0036]
计算时将球形机器人视为一个点，一般可以选择中心点或质心点。航向角为航向向量与正北方向(y轴)的夹角。
[0037]
本发明对于能够用参数式方程表示的期望路径曲线，例如x(t)和y(t)，在已经通过差分gps获得较为精确的水面球形机器人当前的位置坐标的前提下，可以求得机器人距离该曲线(也就是期望巡线的路径)的最小距离、最小距离对应在曲线上的点(简称最小距离点)，以及其对应的参数t值。在机器人距离曲线较远时，要求机器人航向角调整至朝向最小距离点处，并往该处行进；当机器人到曲线的最短距离中等时，会要求机器人的期望航向角调整至朝向最小距离点往前一定距离(简称前向距离)的位置，这么处理的目的是，让水面球形机器人能够非常平滑的逐渐接近期望路径，从而避免中途刹车再转向或者过冲超调的低效情况发生，当机器人距离曲线的最短距离非常小时，会在计算前向距离的基础上施加前向距离的偏移量，目标是为了在该前向距离的牵引下，最小距离能始终处于0，从而达到曲线路径跟踪的效果。
[0038]
本方案在设计期望航向角时不采用期望圆半径的方法，而是引入了前向距离的概念，实现曲线路径跟踪，其方法比现有曲线路径跟踪方法(如虚拟船舶指导法和基于模型的反步法等方法)在设计架构上更加简单，且不依赖于模型因此对于各种不同的曲线方程和不同的船体都具有良好的鲁棒性，通过使用非线性控制算法s面控制器对球形机器人临近期望路径时的前向距离进行进一步优化，使得机器人在逼近期望路径时能有非常平滑的逼近效果。本方案可以很容易应用在舰船和飞机等非球体构造的移动机器人中，这是本方案的又一大优势。
附图说明
[0039]
图1是本发明的一种结构示意图。
具体实施方式
[0040]
下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
[0041]
实施例：本实施例的一种基于自适应变前向距离的球形机器人水面运动控制方法，如图1所示，包括以下步骤：
[0042]
s1、校准航向角零位，启动脚本接收imu(inertial measurement unit，惯性测量单元)数据；本方法基于的航向角是从正北方向开始算起，根据东北地坐标和右手系法则，顺时针为航向角正方向，但是imu测得的初始航向零位为启动imu数据接收脚本的瞬间的球的朝向，所以需要校准航向角零位后，再启动相应脚本；
[0043]
s2、对imu反馈的航向角做坐标变换，变为东北地坐标系；imu航向角基于东北天坐标系，所以需要进行坐标变换；
[0044]
s3、读取设置好的球形机器人的期望路径；期望路径用事先给定的两个参数式方程x(t)和y(t)表示，为曲线路径；
[0045]
s4、接收来自imu和启动rtk(real time kinematic，实时动态测量技术)后的gps数据作为状态测量值，根据当前状态测量值和期望路径，利用自适应变前向距离算法得到球形机器人的当前的航向角与期望的航向角的偏差；
[0046]
s5、根据球形机器人的当前的航向角与期望的航向角的偏差，计算期望角速度；
[0047]
s6、球形机器人的推力分配单元依照期望角速度分配左右螺旋桨的转速，使航向角和路径跟踪误差收敛；
[0048]
s7、判断球形机器人是否走完跟踪路径，如果已经走完，航向控制结束；如果没有走完跟踪路径，重复执行步骤s2到步骤s6，直至走完路径。
[0049]
所述步骤s4中，利用自适应变前向距离算法得到球形机器人的当前航向角与期望的航向角的偏差的步骤具体为：
[0050]
s401、获得球形机器人与期望路径的最小距离、最小距离点以及t1；最小距离是指球形机器人与期望路径的最小距离，最小距离点为期望路径上距离球形机器人最近的点(x(t1),y(t1))，最小距离是指最小距离点和球形机器人之间的距离，t1是是指最小距离点用两个参数式方程表现时对应的参数t值；
[0051]
s402、计算当前航向角和期望路径在t1位置处的切向量夹角e
psi
；
[0052]
s403、当最小距离大于当前航速10秒内的移动距离时，前向距离设置为0；
[0053]
当最小距离小于10倍航速数值之间的距离时，通过以下公式确定前向距离val：
[0054][0055]
式中，a、b和c为经验参数；
[0056]
s404、将前向距离转化为参数t的变化量dt值，dt所对应的弧长为d，计算弧长d对应的参数t2的值；即t2＝t1+dt，这里的参数均指两个参数式方程x(t)和y(t)的参数；
[0057]
可以通过一种数值方法(即积分)来逼近弧长的计算。在计算曲线的弧长时，如果把曲线分成很多小段，每段都近似为直线，然后简单地把这些直线段的长度加起来。这种方法通常称为数值积分。这种近似方法可以极大简化计算。本专利使用这种近似来计算弧长。通过增量地增加参数t，并计算相邻点之间的距离，可以估计出曲线上的弧长。这种方法在t的增量很小的情况下通常是有效的，因为在这些小区间内，曲线的形状可以近似为直线。
[0058]
s405、计算当前球形机器人所处位置到前向距离点所连向量与y轴夹角，即为期望航向角，通过相减获得当前航向角与期望的航向角的偏差。
[0059]
前向距离点的定义：参数式曲线方程中将t2代入t所得到的坐标点称为前向距离点。t2对应的是从t1沿着t增大的方向经过前向距离长度的弧长所达到的坐标点，故称为前向距离点。在求得从机器人所处位置到t2位置所连向量以后，可以通过余弦公式求得与y轴夹角，然后，计算这个向量与y轴的夹角，即为下一时刻期望航向角，求取方法与前述曲线切向量与y轴夹角的求法相同。
[0060]
即在水面球形机器人距离曲线较近时，如果e
psi
较大，说明球形机器人更像是以倾向于90度(甚至更大)的角度冲向曲线，则此时应该把前向距离调大，让球形机器人以更加平滑的角度逐渐趋近期望路径。如果e
psi
较小，说明球形机器人的朝向已经与路径的切线方向比较吻合，此时可以适当减小前向距离。此时减小前向距离的好处有两个，其一是当球形机器人还未正式抵达期望路径时，可以以更高的逼近速度趋向期望路径；其二是当球形机器人已经抵达期望路径时，较小的前向距离能够让球形机器人更好地应对曲率较大的情况。因此，e
psi
和前向距离大体成正比关系。
[0061]
步骤s403中，当最小距离小于2倍航速数值之间的距离时，确定前向距离后，采用s面控制算法对前向距离进行修正：
[0062][0063]
z为前向距离的偏移量，将z叠加到原有的前向距离上完成修正，k1和k2为参数，g代表最小距离与期望最小距离的偏差归一化处理后的值，代表g的变化率归一化处理后的值，由偏差对采样时间间隔作商求得。典型值是k1为4，k2为0.8，采样时间间隔为0.01秒。
[0064]
在一些特殊情况下(比如曲线为阿基米德螺线等曲率随位置不断变动的复杂曲线)，在水面球形机器人抵达期望路径后，会出现围绕期望路径做正弦波状振荡的现象，平均横向偏差通常不超过1米，但是显然显著降低了航向效率。原因在于曲率变化较快，球形机器人的前向距离过小导致仍然存在不可忽视的过冲现象。因此，有必要对最小距离min_dist很小的情况进一步优化前向距离。故采用s面控制算法来对dt做额外偏移量。
[0065]
前向距离定义为从最小距离点开始，沿着曲线参数t增加的方向增长的弧长长度。前向距离的选取非常关键也存在难度。一方面，如果前向距离过近，有很大可能会使球形机器人在接近期望路径时出现过冲；另一方面，如果前向距离过大，很容易会让球形机器人的收敛到期望路径的速度过慢。而本方案既可以避免出现过冲，也可以具有较好的收敛速度。
[0066]
步骤s401中，最小距离由以下方式求得：
[0067]
在一个指定的有限区间内，首先尝试使用抛物线插值法寻求最小值，如果失败，则使用黄金分割法寻求最小距离。
[0068]
实际应用时，求最小距离的过程封装为一个代码函数，有限区间在调用代码函数时指定；通过调用代码函数，输入指定曲线参数t区间和待求最小值的函数，直接获得最小距离、最小距离点以及对应的参数值t1。
[0069]
距离函数为：
[0070]
[0071]
(x1,y1)为球形机器人坐标。
[0072]
计算e
psi
时，需要先求t1位置处曲线的切线方向向量(方向朝向参数t增大的方向)与y轴的夹角，切线斜率的求取使用有限差分方法。有限差分方法可以以近似数值计算的方式较为快速地求得曲线的切线斜率。具体来说，就是定义切线方向向量为一个指向t增大方向的二维向量(dx/dt,dy/dt)。这个向量的y分量(dy/dt)与x分量(dx/dt)的比值就是切线斜率。然后，计算这个向量与y轴的夹角。对于y轴，其方向向量可以认为是(0,1)。利用向量的点乘公式来计算夹角，即：
[0073][0074]
在这里，a是切线方向向量，b是y轴的方向向量。注意到向量b的模长为1，所以公式可以简化为：
[0075][0076]
再对余弦值作反余弦变换，就可以返回的夹角的度数(即为切向量与y轴的夹角)，将其与当前球形机器人的航向角作差，即得到当前航向角psi和曲线在t1位置处的切向量夹角e
psi
。
[0077]
步骤s5中，期望角速度通过pid控制的方式得到。
[0078]
在使用pid控制让机器人的实际角速度跟踪期望角速度时，控制器将计算期望角速度和实际角速度之间的差距，并根据这个差值来调整控制输入。pid控制器由三个部分组成：比例(p)，积分(i)和微分(d)。比例(p)部分是误差的简单倍数。比例增益(kp)决定了响应的强度。如果误差很大，比例项将增加控制输入，以更快地减少误差。但是，仅使用比例控制通常会导致超调。积分(i)部分是误差随时间的累积和。在角速度控制时，积分增益(ki)帮助消除由于环境扰动因素引起的稳态误差。当机器人的角速度接近目标但由于扰动(可能是持续性扰动)而不能达到期望值时，积分项可以提供额外的控制输入。微分(d):微分部分是误差的变化率。微分增益(kd)有助于预测系统的行为，并减少超调和振荡。当实际角速度快速接近其目标角速度时，微分项可以减少控制输入，使其更平滑地接近目标。
[0079]
此外，还可以采用lqr、滑膜控制、模糊控制、s面控制等方式来获得期望角速度。
[0080]
计算时将球形机器人视为一个点，一般可以选择中心点或质心点。航向角为航向向量与正北方向(y轴)的夹角。
[0081]
还可以通过s面航向控制算法来获得期望角速度，具体公式为：
[0082][0083]
式中，z代表下发的期望角速度值，k3和k4为控制参数，h为期望航向角和当前航向角的偏差经过归一化处理后的值，为期望航向角和当前航向角的偏差率经过归一化处理后的值。
[0084]
对于输入为偏差和偏差率(双变量)时，输入-输出对应的是一个由sigmoid函数衍生的曲面，简称s面，其中k3和k4代表要调节的参数。s面控制算法理论上可以用来调节舰船
的任一自由度。如果是对于欠驱动的球形机器人，s面控制算法则可以用来调节航向角和速度。可以看到，s面控制算法中待调节的参数和pd控制一样也是2个，分别为k3和k4，其中k3代表比例项，以调节航向角为例，k3越大，航向角响应速度越快，但实际航向角超调就会越大；k4代表微分项，k4越大，响应越平稳，超调越小，但是可能会放大电路中的噪声和杂波。k3和k4需要经过实地调试确定，但从定性上分析可知，k3和k4需要同号。调节k3和k4的本质是让控制输出在控制面上的梯度方向尽可能逼近指向平衡点(零点)的方向。s面控制算法中的输入需要经过归一化处理。
[0085]
s面控制算法的控制参数由以下公式确定：
[0086][0087][0088]
如果或或
[0089]
式中，u1(k)和u2(k)分别代表当前k时刻s面控制器中的k3和k4参数；λ为权重因子，λ》0；μ为惩罚因子，μ》0；η为第一步长因子，η∈(0,1]；ρ为第二步长因子，ρ∈(0,1]；ε为充分小正数；为φ1(k)的估计值；为φ2(k)的估计值；φ1(k)和φ2(k)为时变标量参数；为的初始值，为的初始值，这两个初始值均由人为设定；δu1(k-1)＝u1(k-1)-u1(k-2)，即k-2时刻到k-1时刻的k3参数增量δk3，δu2(k-1)＝u2(k-1)-u2(k-2)，即k-2时刻到k-1时刻的k4参数增量δk4；y
*
(k+1)代表期望航向角(面向下一个时刻，因此变量是k+1)，y(k)代表当前航向角；δy(k)＝y(k)-y(k-1)，y(k-1)为上一个采样时刻得到的航向角。
[0090]
权重因子、惩罚因子、第一步长因子和第二步长因子均为经验参数，由人为事先设定；这些参数一经设定，就不需要再变动，另一方面，这些因子经过粗调就可以很容易定下，虽然不一定是最佳取值，但是对于更换了的新的非线性系统和动态的外部干扰环境不敏感，即对于最终效果没有决定性影响，仅仅只是影响了和的起作用的幅度大小，但是对非线性系统的跟踪和对时变环境的适应都是依靠和来完成的。
[0091]
公式(2)中求得和后，通过公式(3)进一步优化，即公式(3)引入了“算法重置”机制，以增强公式(2)对时变标量参数的跟踪性能。
[0092]
公式(3)中第一个条件和第二个条件都是表示在变化极小的时候要进行重置，因为当不论k1还是k2出现变化极小的情况时，要么原因可能是出现了稳态，要么可能是预测信息出现了不准确，如果是第二种原因并一直任由原先的参数进展下去，可能会逐渐增加预测误差。为了排除是第二个原因导致的，通过重置到初始值可以有助于系统重新适应可能
的变化。至于第三个条件，是因为这个控制方案在理论上如果能够确保控制系统始终是稳定的，前提必须要满足系统“拟线性”的假设，这个假设的等价表述是：对任意时刻k及
△
u(k)不等于0，和的符号保持不变。所以一旦或出现符号的变化，即和初始符号相反，就有必要对和重置，这样这个控制方案才始终可以满足可设计求解的必要条件。
[0093]
本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
[0094]
尽管本文较多地使用了期望航向角、期望角速度、s面控制算法等术语，但并不排除使用其它术语的可能性。使用这些术语仅仅是为了更方便地描述和解释本发明的本质；把它们解释成任何一种附加的限制都是与本发明精神相违背的。

技术特征：
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位置处的切向量夹角e
psi
；s403、当最小距离大于当前航速10秒内的移动距离时，前向距离设置为0；当最小距离小于10倍航速数值之间的距离时，通过以下公式确定前向距离val：式中，a、b和c为经验参数；s404、将前向距离转化为参数t的变化量dt值，dt所对应的弧长为d，计算弧长d对应的参数t2的值；s405、计算当前球形机器人所处位置到前向距离点所连向量与y轴夹角，即为期望航向角，通过相减获得当前航向角与期望的航向角的偏差。3.根据权利要求2所述的基于自适应变前向距离的球形机器人水面运动控制方法，其特征在于，步骤s403中，当最小距离小于2倍航速数值之间的距离时，确定前向距离后，采用s面控制算法对前向距离进行修正：式中，z为前向距离的偏移量，将z叠加到原有的前向距离上完成修正，k
1-和k2为参数，g代表最小距离与期望最小距离的偏差归一化处理后的值，代表g的变化率归一化处理后
的值，由偏差对采样时间间隔作商求得。4.根据权利要求1或2所述的基于自适应变前向距离的球形机器人水面运动控制方法，其特征在于，步骤s401中，最小距离由以下方式求得：在一个指定的有限区间内，首先尝试使用抛物线插值法寻求最小值，如果失败，则使用黄金分割法寻求最小距离。5.根据权利要求3所述的基于自适应变前向距离的球形机器人水面运动控制方法，其特征在于，步骤s5中，期望角速度通过pid控制的方式得到。

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


技术研发人员：王酉 曹勖之 邹虹群
受保护的技术使用者：逻腾（杭州）科技有限公司
技术研发日：2023.07.06
技术公布日：2023/9/7