Page 50 - 《软件学报》2025年第8期

张善强 等: 单球驱动平衡机器人运动学和动力学形式化验证 3473

π
2
令 x= , 则 sin(x)=1, cos(x)=0, 此时上式可化简为 2γ ¨x+β¨x −2µ+2τ = 0. 由于 β, γ, µ 均是常数, 令 β = 1, γ = 1,
2
2 τ = 1.
µ = 1, 此时有 2¨x+ ¨x −2 = −2τ. 若 ¨ x = −2, 则

4.3 XOY 面动力学方程形式化建模与验证
在第 4.2 节中, 验证了 XOZ 面的动力学方程的正确性. 为了提高单球驱动平衡机器人控制的精度, 将继续验
证 XOY 面的动力学方程的正确性. XOY 面的平面模型如图 5 所示.

δ

l′
φ

图 5 XOY 面平面模型图

图 5 中, φ 表示球轮的旋转角, δ 表示机身的旋转角, l 表示机身质心与球轮中心的水平距离. 选取 φ 和 作为
δ
′
广义坐标, 建立系统的拉格朗日方程. 对应的拉格朗日量如下:

1 1
2
L = I b ˙φ + I t δ ˙ 2 (10)
2 2
将拉格朗日量代入拉格朗日方程, 得到单球平衡机器人 XOY 面对应的拉格朗日动力学方程. 如下所示:

    
 I b 0  ¨φ   0 
    
    τ (11)

   = 
  ¨ δ   
0 I t 1
其中, I b 表示球轮的转动惯量, I t 表示机身的转动惯量, τ 表示对应的扭矩.
对公式 (11) 进行形式化描述, 如定理 5 所示.
定理 5. XOY 面的拉格朗日动力学方程.
!q t0 t1 Mt It Ib l r phi phi phi delta delta delta q q q t.
′
′′
′
′
′′
′′

[A1]: drop t0 < drop t1/\interval[t0, t1] = s/\t IN s/\
∧
[A2]: {u : real (2,1)finite_sum|?y.y IN mspace(fspace s)/\u IN IMAGE y s} = s0/\
{u : real (2,2)finite_sum|?y.y IN mspace(fspac s)/\u IN IMAGE y s} = s1/\
∧
[A3]: q = (\t.vector[drop(phi t);drop(delta t)] : real 2)/\
∧
q = (\t.vector[drop(phi t);drop(delta t)] : real 2)/\
′
′
′
∧
q = (\t.vector[drop(phi t);drop(delta t)] : real 2)/\
∧
′′
′′
′′
′
[A4]: (!t.t IN s ==> (phi has_vector_derivative phi t)(at t within s))/\
(!t.t IN s ==> (delta has_vector_derivative delta t)(at t within s))/\
′
′′
′
(!t.t IN s ==> (phi has_vector_derivative phi t)(at t within s))/\
(!t.t IN s ==> (delta has_vector_derivative delta t)(at t within s))/\
′′
′
[A5]: ke = (\u : real (2,2)finite_sum.lift((&1/&2∗ Ib∗sndcart u$1∗sndcart u$1)+
∧
(&1/&2∗ It ∗sndcart u$2∗sndcart u$2)))/\
ue = (\u : real 2.lift(&0))/\
∧
[A6]: alpha = Ib+(Mb+ Mt)∗r ∗r/\
beta = Mt ∗r ∗l/\
gamma = It + Mt ∗l∗l/\mu = Mt ∗g∗l/\
mu = Mt ∗g∗l/\