一种基于动力学的两轮自平衡轮椅的非线性控制方法

本发明涉及一种用于两轮自平衡轮椅的非线性控制方法，属机器人控制领域。背景技术：：：1、目前，随着老年人和残疾人数量的不断增加，轮椅作为代步工具受到广泛关注。普通轮椅需要使用者自己转动把手或者需要他人帮助使用，使用者行动受限、较为不便，因此安全、智能化的轮椅就显得格外重要。相较普通四轮轮椅，两轮自平衡轮椅体积更小，两个大轮具有很强的地面适应能力和更小的转弯半径，可以满足使用者独立、稳定、安全的出行需求。2、两轮自平衡轮椅需要解决自平衡控制问题，目前的自平衡技术已经较为成熟，大多通过pid和lqr控制算法实现自平衡及运动功能。thao ng等人（thao ng, nghia dh,phuc nh. a pid backstepping controller for two-wheeled self-balancing robot.ininternational forum on strategic technology 2010 2010 oct 13 (pp. 76-81).ieee.）设计pid反步控制器包括三个控制回路，分别控制机器人的平衡、位置、运动方向，pid控制是无模型的线性控制器，在非线性系统中存在局限性。lqr控制方法基于系统的线性模型建立，jian fang等人（jian fang, "the lqr controller design of two-wheeledself-balancing robot based on the particle swarm optimization algorithm",mathematical problems in engineering, vol. 2014, article id 729095, 6 pages,2014. https://doi.org /10.1155/2014/729095）采用基于线性化倒立摆模型的lqr控制方法提高系统稳定性，在角度较小情况下控制算法有效，但是其在非线性系统中有很大局限性，倾斜角度超出一定范围，控制算法失效。技术实现思路1、本发明的目的在于克服现有技术不足，提供一种以机器人动力学为约束的两轮自平衡轮椅非线性模型预测控制方法。首先建立两轮自平衡轮椅的非线性动力学方程，针对两轮自平衡轮椅的状态方程，构造连续时间的非线性模型预测控制问题作为轨迹规划器需求解的问题。非线性模型预测控制问题是一个连续时间的最优控制问题，它的求解是一个复杂问题，通过直接转录法将其离散化为普通的非线性规划问题，进而求解非线性规划问题得到最优状态轨迹轨迹和最优控制轨迹，选择最优状态轨迹的初始部分作为接下来一段时间内两轮自平衡轮椅运动过程中状态的预测，采取pd控制器进行最优状态轨迹跟踪从而完成控制，减小实时的误差，使得两轮自平衡轮椅能够更快速、更稳定完成期望的运动。2、本发明的技术方案：3、一种基于动力学的两轮自平衡轮椅的非线性控制方法，步骤如下：4、步骤1：建立两轮自平衡轮椅的运动学方程；5、两轮自平衡轮椅的运动学方程由拉格朗日建模方法得到，表达式为：6、(1.1)7、其中，<mstyledisplaystyle="true"mathcolor="#000000"><mi>q</mi><mi>=</mi><msup><mrow><mo>[</mo><mtable><mtr><mtd><mi>y</mi></mtd><mtd><mi>θ</mi></mtd><mtd><msub><mi>α</mi><mi>r</mi></msub></mtd><mtd><msub><mi>α</mi><mi>l</mi></msub></mtd></mtr></mtable><mo>]</mo></mrow><mi>t</mi></msup></mstyle>表示两轮自平衡轮椅的广义坐标，表示的一阶导数，表示的二阶导数，为两轮自平衡轮椅的位移，为两轮自平衡轮椅的俯仰角，为两轮自平衡轮椅的右轮的转角，为两轮自平衡轮椅的左轮的转角，表示两轮自平衡轮椅的惯性矩阵，描述两轮自平衡轮椅的质量分布和惯性特性，表示科氏力矩阵与重力矩阵的和，科氏力是由车轮速度和车轮与座椅之间相互作用力产生的，表示作用于两轮自平衡轮椅的车轮的力矩，的表达式为：8、<mstyledisplaystyle="true"mathcolor="#000000"><mi>b</mi><mi>=</mi><mrow><mo>[</mo><mtable><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mi>−</mi><mn>1</mn></mtd><mtd><mi>−</mi><mn>1</mn></mtd></mtr><mtr><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd></mtr></mtable><mo>]</mo></mrow></mstyle>(1.2)9、步骤2：将两轮自平衡轮椅的运动学方程通过与运动学约束矩阵的零空间映射矩阵相乘得到状态方程；具体如下：10、由于两轮自平衡轮椅是具有非完整约束的系统，使用众所周知的基于状态空间的控制方法之前，需要考虑系统中存在的约束条件将其加入运动学方程中。在驱动轮不打滑情况下，两轮自平衡轮椅的运动学约束条件为：11、(1.3)12、其中，表示车轮的半径，表示两轮自平衡轮椅的速度，公式(1.3)中的两个表达式写为矩阵形式：13、<mstyledisplaystyle="true"mathcolor="#000000"><mtablecolumnalign="center"><mtr><mtd><mi>a</mi><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><moveraccent="true"><mi>q</mi><mo>˙</mo></mover><mi>=</mi><mn>0</mn></mtd></mtr><mtr><mtd><mi>a</mi><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>=</mi><mrow><mo>[</mo><mtable><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mi>−</mi><mn>1</mn></mtd></mtr><mtr><mtd><mn>2</mn></mtd><mtd><mn>0</mn></mtd><mtd><mi>−</mi><mi>r</mi></mtd><mtd><mi>−</mi><mi>r</mi></mtd></mtr></mtable><mo>]</mo></mrow></mtd></mtr></mtable></mstyle>(1.4)14、为运动学约束矩阵，m、n分别表示矩阵的行、列数。15、引入拉格朗日乘子来表示运动学约束的大小，将运动学约束条件与运动学方程(1.1)相关联得到两轮自平衡轮椅的动力学方程为：16、(1.5)17、定义一个满秩矩阵，满秩矩阵是运动学约束矩阵的零空间映射矩阵，即，其中，n为广义坐标中元素的个数，表示运动学约束条件的个数：18、<mstyledisplaystyle="true"mathcolor="#000000"><mi>s</mi><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>=</mi><mrow><mo>[</mo><mtable><mtr><mtd><mi>r</mi></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd></mtr><mtr><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd></mtr></mtable><mo>]</mo></mrow></mstyle>(1.6)19、定义一个伪速度向量为，则：20、(1.7)21、对式(1.7)求导并代入两轮自平衡轮椅的动力学方程(1.5)中：22、(1.8)23、由于，因此在式子（1.8）两边乘以来消除拉格朗日乘子得到最终的两轮自平衡轮椅的动力学方程：24、(1.9)25、由于和与广义坐标中其他变量完全解耦，剩余变量构成一个新的向量<mstyledisplaystyle="true"mathcolor="#000000"><msub><mi>q</mi><mi>r</mi></msub><mi>=</mi><msup><mrow><mo>[</mo><mtable><mtr><mtd><mi>y</mi></mtd><mtd><mi>θ</mi></mtd></mtr></mtable><mo>]</mo></mrow><mi>t</mi></msup></mstyle>作为实际的控制变量。定义状态为<mstyledisplaystyle="true"mathcolor="#000000"><mi>x</mi><mi>=</mi><msup><mrow><mo>[</mo><mtable><mtr><mtd><msub><mi>q</mi><mi>r</mi></msub></mtd><mtd><msub><moveraccent="true"><mi>q</mi><mo>˙</mo></mover><mi>r</mi></msub></mtd></mtr></mtable><mo>]</mo></mrow><mi>t</mi></msup></mstyle>，推导得出状态方程：26、<mstyledisplaystyle="true"mathcolor="#000000"><moveraccent="true"><mi>x</mi><mo>˙</mo></mover><mi>=</mi><mrow><mo>[</mo><mtable><mtr><mtd><moveraccent="true"><mi>y</mi><mo>˙</mo></mover></mtd></mtr><mtr><mtd><moveraccent="true"><mi>θ</mi><mo>˙</mo></mover></mtd></mtr><mtr><mtd><msup><mrow><mo>[</mo><mrow><msup><mi>s</mi><mi>t</mi></msup><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>m</mi><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>s</mi><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow></mrow><mo>]</mo></mrow><mrow><mi>−</mi><mn>1</mn></mrow></msup><mrow><mo>[</mo><mrow><msup><mi>s</mi><mi>t</mi></msup><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>b</mi><mi>τ</mi><mi>−</mi><msup><mi>s</mi><mi>t</mi></msup><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>m</mi><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><moveraccent="true"><mi>s</mi><mo>˙</mo></mover><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>x</mi><mi>−</mi><msup><mi>s</mi><mi>t</mi></msup><mrow><mo>(</mo><mi>q</mi><mo>)</mo></mrow><mi>h</mi><mrow><mo>(</mo><mrow><mi>q</mi><mi>,</mi><moveraccent="true"><mi>q</mi><mo>˙</mo></mover></mrow><mo>)</mo></mrow></mrow><mo>]</mo></mrow></mtd></mtr></mtable><mo>]</mo></mrow></mstyle>(1.10)27、步骤3：构造一个非线性模型预测控制问题，非线性模型预测控制问题是一个最优控制问题，包括成本函数和约束条件，成本函数写成积分形式：28、(1.11)29、其中，为运动时间，为运动的初始时刻，为运动的最终时刻；为连续时间的状态轨迹，为连续时间的力矩轨迹，为被积分函数，将力矩轨迹最小、状态轨迹的偏差最小作为优化目标，被积函数写为：30、(1.12)31、其中，为力矩轨迹权重参数，为状态轨迹权重参数，为最终时刻期望的状态。决策变量为初始时刻和最终时刻、两轮自平衡轮椅的连续时间的状态轨迹和连续时间的力矩轨迹，通过计算寻找出决策变量的值使得成本函数达到最小值。32、把两轮自平衡轮椅的状态方程作为约束条件，保证结果符合两轮自平衡轮椅的动力学方程。同时，对初始时刻和最终时刻、两轮自平衡轮椅的连续时间的状态轨迹和连续时间的力矩轨迹、初始时刻的状态和最终时刻的状态进行约束：33、(1.13)34、其中，为连续时间的状态方程，可以表示为包含时间、状态轨迹和力矩轨迹的函数形式；和分别表示初始时刻和最终时刻的最小值和最大值；和分别表示状态轨迹中状态的最小值和最大值；和分别表示力矩轨迹中力矩的最小值和最大值；和分别表示初始时刻的状态的最小值和最大值，和分别表示最终时刻的状态的最小值和最大值。35、步骤4：求解步骤3构造的非线性模型预测控制问题，将问题本身离散化，即将一个连续时间的非线性最优控制问题转为非线性规划问题。36、（4.1）设置离散网格；37、设离散网格数为n，连续时间离散为n个时间节点，第个时间节点为，其中，为预测的时间窗口长度，两个连续时间节点的时间间隔为。38、（4.2）对成本函数进行离散化；39、对于成本函数，计算每个时间节点的间隔之间的成本函数，将成本函数近似为求和函数。将被积函数近似为二次函数，离散后的初始时刻和第一个时间节点之间的时间间隔的成本函数表达式为：40、(1.14)41、初始时刻为，选取三点，，，为初始时刻和第一个时间节点之间的中点时刻，得出二次函数的系数为：42、(1.15)43、将系数代入成本函数得到第一段时间间隔的成本函数为：44、(1.16)45、以此类推，离散化的成本函数为初始时刻到最终时刻之间的每一段间隔的成本函数之和：46、(1.17)47、（4.3）对连续时间状态方程进行离散化；48、对公式（1.13）中约束条件中的连续时间的状态方程进行离散化，将连续时间的状态方程写成积分形式，用上述方法逼近积分来离散化状态方程：49、(1.18)50、其中，为离散化的第个时间节点的状态，是两个时间节点的中点时刻的函数形式的状态方程，两个时间节点的中点时刻的状态未知，通过二次插值基函数方法计算两个时间节点和之间的状态轨迹为：51、 (1.19)52、将两个连续时间节点的中点时刻代入式（1.19），其中，中点时刻为，两节点的时间间隔为。计算得出中点时刻的状态并化简：53、(1.20)54、（4.4）对连续时间的状态轨迹进行离散化，状态轨迹离散化为n个时间节点的状态，第个时间节点的状态约束为：55、(1.21)56、（4.5）对连续时间的力矩轨迹进行离散化，第个时间节点的力矩约束为：57、(1.22)58、（4.6）初始时刻和最终时刻的状态满足约束：59、<mstyledisplaystyle="true"mathcolor="#000000"><msub><mi>x</mi><mn>0</mn></msub><mi>=</mi><mrow><mo>[</mo><mrow><msub><mi>y</mi><mn>0</mn></msub><mi>,</mi><msub><mi>θ</mi><mn>0</mn></msub><mi>,</mi><msub><moveraccent="true"><mi>y</mi><mo>˙</mo></mover><mn>0</mn></msub><mi>,</mi><msub><moveraccent="true"><mi>θ</mi><mo>˙</mo></mover><mn>0</mn></msub></mrow><mo>]</mo></mrow><mi>，</mi><msub><mi>x</mi><mi>n</mi></msub><mi>=</mi><mrow><mo>[</mo><mrow><msub><mi>y</mi><mi>n</mi></msub><mi>,</mi><msub><mi>θ</mi><mi>n</mi></msub><mi>,</mi><msub><moveraccent="true"><mi>y</mi><mo>˙</mo></mover><mi>n</mi></msub><mi>,</mi><msub><moveraccent="true"><mi>θ</mi><mo>˙</mo></mover><mi>n</mi></msub></mrow><mo>]</mo></mrow></mstyle>(1.23)60、其中，，，和分别是两轮自平衡轮椅初始时刻的位移、俯仰角度、速度、俯仰角速度；，，和分别是两轮自平衡轮椅最终时刻的位移、俯仰角度、速度、俯仰角速度。61、综上，最优控制问题离散化为非线性规划问题：62、<mstyledisplaystyle="true"mathcolor="#000000"><mtablecolumnalign="left"><mtr><mtd><munder><mi>min</mi><mtablecolumnalign="left"><mtr><mtd><msub><mi>x</mi><mn>0</mn></msub><mi>,</mi><mi>…</mi><msub><mi>,x</mi><mi>i</mi></msub></mtd></mtr><mtr><mtd><msub><mi>τ</mi><mn>0</mn></msub><mi>,</mi><mi>…</mi><msub><mi>,τ</mi><mi>i</mi></msub></mtd></mtr></mtable></munder><mstyledisplaystyle="true"><munderover><mo>∑</mo><mrow><mi>i</mi><mi>=</mi><mn>0</mn></mrow><mrow><mi>n</mi><mi>−</mi><mn>1</mn></mrow></munderover><mrow><mfrac><msub><mi>ς</mi><mi>i</mi></msub><mn>6</mn></mfrac><mrow><mo>(</mo><mrow><msub><mi>g</mi><mi>i</mi></msub><mo>+</mo><mn>4</mn><msub><mi>g</mi><mrow><mi>i</mi><mo>+</mo><mfrac><mn>1</mn><mn>2</mn></mfrac></mrow></msub><mo>+</mo><msub><mi>g</mi><mrow><mi>i</mi><mo>+</mo><mn>1</mn></mrow></msub></mrow><mo>)</mo></mrow></mrow></mstyle></mtd></mtr><mtr><mtd><mi>s</mi><mi>.</mi><mi>t</mi><mi>.</mi><mrow><mo>{</mo><mtablecolumnalign="left"><mtr><mtd><msub><mi>x</mi><mrow><mi>i</mi><mo>+</mo><mn>1</mn></mrow></msub><mi>−</mi><msub><mi>x</mi><mi>i</mi></msub><mi>=</mi><mfrac><msub><mi>ς</mi><mi>i</mi></msub><mn>6</mn></mfrac><mrow><mo>(</mo><mrow><msub><mi>f</mi><mi>i</mi></msub><mo>+</mo><msub><mi>f</mi><mrow><mi>i</mi><mo>+</mo><mfrac><mn>1</mn><mn>2</mn></mfrac></mrow></msub><mo>+</mo><msub><mi>f</mi><mrow><mi>i</mi><mo>+</mo><mn>1</mn></mrow></msub></mrow><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>x</mi><mrow><mi>i</mi><mo>+</mo><mfrac><mn>1</mn><mn>2</mn></mfrac></mrow></msub><mi>=</mi><mfrac><mn>1</mn><mn>2</mn></mfrac><mrow><mo>(</mo><mrow><msub><mi>x</mi><mi>i</mi></msub><mo>+</mo><msub><mi>x</mi><mrow><mi>i</mi><mo>+</mo><mn>1</mn></mrow></msub></mrow><mo>)</mo></mrow><mo>+</mo><mfrac><msub><mi>ς</mi><mi>i</mi></msub><mn>8</mn></mfrac><mrow><mo>(</mo><mrow><msub><mi>f</mi><mi>i</mi></msub><mi>−</mi><msub><mi>f</mi><mrow><mi>i</mi><mo>+</mo><mn>1</mn></mrow></msub></mrow><mo>)</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>x</mi><mi>low</mi></msub><mi>≤</mi><msub><mi>x</mi><mi>i</mi></msub><mi>≤</mi><msub><mi>x</mi><mi>up</mi></msub></mtd></mtr><mtr><mtd><msub><mi>τ</mi><mi>low</mi></msub><mi>≤</mi><msub><mi>τ</mi><mi>i</mi></msub><mi>≤</mi><msub><mi>τ</mi><mi>up</mi></msub></mtd></mtr><mtr><mtd><msub><mi>x</mi><mn>0</mn></msub><mi>=</mi><mrow><mo>[</mo><mrow><msub><mi>y</mi><mn>0</mn></msub><mi>,</mi><msub><mi>θ</mi><mn>0</mn></msub><mi>,</mi><msub><moveraccent="true"><mi>y</mi><mo>˙</mo></mover><mn>0</mn></msub><mi>,</mi><msub><moveraccent="true"><mi>θ</mi><mo>˙</mo></mover><mn>0</mn></msub></mrow><mo>]</mo></mrow></mtd></mtr><mtr><mtd><msub><mi>x</mi><mi>n</mi></msub><mi>=</mi><mrow><mo>[</mo><mrow><msub><mi>y</mi><mi>n</mi></msub><mi>,</mi><msub><mi>θ</mi><mi>n</mi></msub><mi>,</mi><msub><moveraccent="true"><mi>y</mi><mo>˙</mo></mover><mi>n</mi></msub><mi>,</mi><msub><moveraccent="true"><mi>θ</mi><mo>˙</mo></mover><mi>n</mi></msub></mrow><mo>]</mo></mrow></mtd></mtr></mtable></mrow></mtd></mtr></mtable></mstyle>(1.24)63、步骤5：两轮自平衡轮椅的实时控制方法；64、（5.1）两轮自平衡轮椅开机后，首先使用pd控制器使两轮自平衡轮椅处于平衡静止状态。65、（5.2）使用者将最终时刻期望的状态指令输入轨迹规划器中，作为非线性规划问题(1.24)最终时刻的状态，初始时刻的状态为0。66、（5.3）轨迹规划器采用内点法二次规划的求解算法求解(1.24)式，从而得到离散的力矩轨迹和离散的状态轨迹的最优解。67、（5.4）轨迹规划器选取状态轨迹最优解的初始部分数据作为接下来一段时间内期望的最优状态轨迹，传输至pd控制器。68、（5.5）两轮自平衡轮椅发送实时的状态信息至pd控制器，pd控制器根据实时的状态信息和步骤（5.4）状态轨迹最优解的初始部分数据所对应每个时刻的状态的偏差来计算力矩。pd控制器将力矩指令传输至两轮自平衡轮椅的电机完成运动。69、（5.6）轨迹规划器完成一次非线性规划问题的求解时，两轮自平衡轮椅同时需要发送实时的状态信息至轨迹规划器，实时的状态信息将作为非线性规划问题(1.24)中的初始时刻的状态，最终时刻状态为使用者输入的最终时刻期望的状态，重复运行（5.3），轨迹规划器再次求解非线性规划问题，如此循环完成闭环控制。70、本发明的有益效果：非线性模型预测控制方法较好完成期望的状态的两轮自平衡轮椅运动，与传统控制方法对比，非线性模型预测控制方法不需要重新调参数且控制效果更好。两轮自平衡轮椅完成由大角度倾倒状态恢复自平衡状态；使用者给定期望的状态指令时，能够快速到达指定状态，能耗相对较小；在受到外力作用下，非线性模型预测控制方法能够使两轮自平衡轮椅快速恢复平衡状态，快速性和稳定性良好。当前第1页12当前第1页12