一种球形机器人水面运动自适应航向控制方法技术

本发明专利技术公开了一种球形机器人水面运动自适应航向控制方法，其包括以下步骤：校准航向角零位，启动脚本接收IMU数据；对IMU反馈的航向角做坐标变换，变为东北地坐标系；读取设置好的球形机器人的期望路径；根据当前状态测量值和期望路径，计算期望航向角；根据当前航向角与期望航向角，利用变参数自适应S面航向控制算法计算期望角速度；球形机器人的角速度控制器根据期望角速度输出力矩和力，使航向角和路径跟踪误差收敛，直至走完路径。本方案可以自适应调整S面航向控制器的两个参数，使得在不同航速和环境干扰下，参数都可以自适应的进行调整以适应这些变化，从而达到鲁棒性更强的航向控制效果。航向控制效果。航向控制效果。

【技术实现步骤摘要】
一种球形机器人水面运动自适应航向控制方法


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

技术介绍

[0002]水陆两栖球形机器人采用的球形结构相较于传统的舰船结构，在水中其转向超调大、控制航向角难度增加，在期望路径存在较大转折时，容易出现超调现象，造成弯折处的路径跟踪误差变大。当前的新产品常常需要花费大量时间来调试新的PID参数，耗时且效率低下，特别是在复杂和变化的环境条件下，例如不同的流速、风速和外挂配重，条件参数控制航向难度非常大。

技术实现思路

[0003]本专利技术主要是解决现有技术所存在的参数调节难度大、控制稳定性差、效率低下等的技术问题，提供一种可以自适应地调整参数适应环境的球形机器人水面运动自适应航向控制方法。
[0004]本专利技术针对上述技术问题主要是通过下述技术方案得以解决的：一种球形机器人水面运动自适应航向控制方法，包括以下步骤：
[0005]S1、校准航向角零位，启动脚本接收IMU(Inertial Measurement Unit，惯性测量单元)数据；本方法基于的航向角是从正北方向开始算起，根据东北地坐标和右手系法则，顺时针为航向角正方向，但是IMU测得的初始航向零位为启动IMU数据接收脚本的瞬间的球的朝向，所以需要校准航向角零位后，再启动相应脚本；
[0006]S2、对IMU反馈的航向角做坐标变换，变为东北地坐标系；IMU航向角基于东北天坐标系，所以需要进行坐标变换；
[0007]S3、读取设置好的球形机器人的期望路径p(σ)；
[0008]S4、接收来自IMU和和启动RTK(Real Time Kinematic，实时动态测量技术)后的GPS数据作为状态测量值，根据当前状态测量值和期望路径，计算期望航向角；
[0009]S5、根据当前航向角与期望航向角，利用变参数自适应S面航向控制算法计算期望角速度；
[0010]S6、球形机器人的角速度控制器根据期望角速度输出力矩和力，使航向角和路径跟踪误差收敛；
[0011]S7、判断球形机器人是否走完跟踪路径，如果已经走完，航向控制结束；如果没有走完跟踪路径，重复执行步骤S2到步骤S6，直至走完路径。
[0012]作为优选，S面航向控制算法为：
[0013][0014]式中，z代表下发的期望角速度值，K1和K2为控制参数，g为期望航向角和当前航向
角的偏差经过归一化处理后的值，为期望航向角和当前航向角的偏差率经过归一化处理后的值。
[0015]对于输入为偏差和偏差率(双变量)时，输入
‑
输出对应的是一个由Sigmoid函数衍生的曲面，简称S面，其中K1和K2代表要调节的参数。S面控制算法理论上可以用来调节舰船的任一自由度。如果是对于欠驱动的球形机器人，S面控制算法则可以用来调节航向角和速度。可以看到，S面控制算法中待调节的参数和PD控制一样也是2个，分别为K1和K2，其中K1代表比例项，以调节航向角为例，K1越大，航向角响应速度越快，但实际航向角超调就会越大；K2代表微分项，K2越大，响应越平稳，超调越小，但是可能会放大电路中的噪声和杂波。K1和K2需要经过实地调试确定，但从定性上分析可知，K1和K2需要同号。调节K1和K2的本质是让控制输出在控制面上的梯度方向尽可能逼近指向平衡点(零点)的方向。S面控制算法中的输入需要经过归一化处理。
[0016]作为优选，S面控制算法的控制参数由以下公式确定：
[0017][0018][0019]如果或或
[0020]式中，u1(k)和u2(k)分别代表当前k时刻S面控制器中的K1和K2参数；λ为权重因子，λ>0；μ为惩罚因子，μ>0；η为第一步长因子，η∈(0,1]；ρ为第二步长因子，ρ∈(0,1]；ε为充分小正数；为φ1(k)的估计值；为φ2(k)的估计值；v1(k)和φ2(k)为时变标量参数；为的初始值，为的初始值，这两个初始值均由人为设定；Δu1(k
‑
1)＝u1(k
‑
1)
‑
u1(k
‑
2)，即k
‑
2时刻到k
‑
1时刻的K1参数增量ΔK1，Δu2(k
‑
1)＝u2(k
‑
1)
‑
u2(k
‑
2)，即k
‑
2时刻到k
‑
1时刻的K2参数增量ΔK2；y
*
(k+1)代表期望航向角(面向下一个时刻，因此变量是k+1)，y(k)代表当前航向角；Δy(k)＝y(k)
‑
y(k
‑
1)，y(k
‑
1)为上一个采样时刻得到的航向角。
[0021]权重因子、惩罚因子、第一步长因子和第二步长因子均为经验参数，由人为事先设定；这些参数一经设定，就不需要再变动，另一方面，这些因子经过粗调就可以很容易定下，虽然不一定是最佳取值，但是对于更换了的新的非线性系统和动态的外部干扰环境不敏感，即对于最终效果没有决定性影响，仅仅只是影响了和的起作用的幅度大小，但是对非线性系统的跟踪和对时变环境的适应都是依靠和来完成的。
[0022]公式(2)中求得和后，通过公式(3)进一步优化，即公式(3)引入了“算法重置”机制，以增强公式(2)对时变标量参数的跟踪性能。
[0023]公式(3)中第一个条件和第二个条件都是表示在变化极小的时候要进行重置，因为当不论K1还是K2出现变化极小的情况时，要么原因可能是出现了稳态，要么可能是预测信息出现了不准确，如果是第二种原因并一直任由原先的参数进展下去，可能会逐渐增加预测误差。为了排除是第二个原因导致的，通过重置到初始值可以有助于系统重新适应可能的变化。至于第三个条件，是因为这个控制方案在理论上如果能够确保控制系统始终是稳定的，前提必须要满足系统“拟线性”的假设，这个假设的等价表述是：对任意时刻k及
△
u(k)不等于0，和的符号保持不变。所以一旦或出现符号的变化，即和初始符号相反，就有必要对和重置，这样这个控制方案才始终可以满足可设计求解的必要条件。
[0024]作为优选，所述步骤S4中，计算期望航向角具体为：
[0025]A1、选取圆弧半径；
[0026]A2、计算圆弧与航线的交点；
[0027]A3、计算期望航向角；
[0028]其中，圆弧半径由以下公式确定：
[0029]R＝
‑
0.5(1
‑
sgn(|y
e
|
‑
R
min
))(R
min
‑
|y
e
|)+|y
e
|
[0030]式中，sgn为符号函数，自变量为正时为1，自变量为负时为
‑
1；y
e...

【技术保护点】

【技术特征摘要】
时刻到k
‑
1时刻的K1参数增量ΔK1，Δu2(k
‑
1)＝u2(k
‑
1)
‑
u2(k
‑
2)，即k
‑
2时刻到k
‑
1时刻的K2参数增量ΔK2；y
*
(k+1)代表期望航向角，y(k)代表当前航向角；Δy(k)＝y(k)
‑
y(k
‑
1)。4.根据权利要求1或2或3所述的一种球形机器人水面运动自适应航向控制方法，其特征在于，所述步骤S4中，计算期望航向角具体为：A1、选取圆弧半径；A2、计算圆弧与航线的交点；A3、计算期望航向角；其中，圆弧半径由以下公式确定：R＝
‑
0.5(1
‑
sgn(|y
e
|
‑
R
min
))(R
min
‑
|y
e
|)+|y
e
|式中，sgn为符号函数，自变量为正时为1，自变量为负时为
‑
1；y
e
为当前船体距离期望路径的横向偏差，在右侧为正，在左侧为负，R
min
为预设的最小圆半径；步骤A2具体为：P<...

【专利技术属性】
技术研发人员：王酉，曹勖之，邹虹群，
申请(专利权)人：浙江大学，
类型：发明
国别省市：