用Newton-Raphson 迭代法求解弹簧阻尼双足机器人在单支撑阶段的不动点的MATLAB

假设弹簧阻尼双足机器人在单支撑阶段的状态方程为：

$\begin{bmatrix}m\ddot{x} \\ m\ddot{y} \\ I\ddot{\theta}\end{bmatrix}=\begin{bmatrix}F_x-F_c \\ F_y-mg \\ M\end{bmatrix}$

其中，$m$ 是机器人的质量，$I$ 是机器人绕垂直于地面的轴的转动惯量，$g$ 是重力加速度，$F_x$ 和 $F_y$ 是地面对机器人的支持力，$F_c$ 是摩擦力，$M$ 是绕垂直于地面的轴的力矩。

在单支撑阶段，机器人的状态向量为 $[x,y,\theta,\dot{x},\dot{y},\dot{\theta}]^T$，其中，$x$ 和 $y$ 分别是机器人的水平和垂直位置，$\theta$ 是机器人的姿态角，$\dot{x}$、$\dot{y}$ 和 $\dot{\theta}$ 分别是机器人在水平、垂直和旋转方向上的速度。

假设机器人的控制输入为 $[F_x,F_y,M]^T$，则状态方程可以写成：

$\begin{bmatrix}\ddot{x} \\ \ddot{y} \\ \ddot{\theta}\end{bmatrix}=\begin{bmatrix}\frac{1}{m}(F_x-F_c) \\ \frac{1}{m}(F_y-mg) \\ \frac{1}{I}M\end{bmatrix}$

其中，摩擦力可以用 Coulomb 摩擦力模型表示：

$F_c=\begin{cases}f_sgn(\dot{x}) & |\dot{x}|<v_s \\ f_ksgn(\dot{x}) & |\dot{x}|\geq v_s\end{cases}$

其中，$f_s$ 和 $f_k$ 分别是静摩擦力和动摩擦力的系数，$v_s$ 是静摩擦力和动摩擦力转换的速度阈值。

在单支撑阶段，机器人的不动点满足 $\dot{x}=\dot{y}=\dot{\theta}=0$，因此，状态方程可以简化为：

$\begin{bmatrix}F_x-F_c \\ F_y-mg \\ M\end{bmatrix}=\begin{bmatrix}0 \\ 0 \\ 0\end{bmatrix}$

将 Coulomb 摩擦力模型代入上式，得到：

$\begin{bmatrix}F_x-f_sgn(\dot{x}) \\ F_y-mg \\ M\end{bmatrix}=\begin{bmatrix}0 \\ 0 \\ 0\end{bmatrix}$

为了使用 Newton-Raphson 迭代法求解不动点，需要将上式表示成向量形式：

$F(\mathbf{x})=\begin{bmatrix}F_x-f_sgn(\dot{x}) \\ F_y-mg \\ M\end{bmatrix}-\begin{bmatrix}0 \\ 0 \\ 0\end{bmatrix}=\begin{bmatrix}F_x-f_sgn(\dot{x}) \\ F_y-mg \\ M\end{bmatrix}$

其中，$\mathbf{x}=[F_x,F_y,M]^T$ 是控制输入向量。

使用 Newton-Raphson 迭代法求解不动点的步骤如下：

1. 初始化控制输入向量 $\mathbf{x}_0$；
2. 计算 $F(\mathbf{x}_0)$ 的值；
3. 如果 $F(\mathbf{x}_0)$ 的范数小于预设的收敛阈值，则输出 $\mathbf{x}_0$ 作为不动点，并结束迭代；
4. 计算 $F(\mathbf{x}_0)$ 的 Jacobian 矩阵 $J(\mathbf{x}_0)$；
5. 计算控制输入向量的更新量 $\Delta\mathbf{x}=-J^{-1}(\mathbf{x}_0)F(\mathbf{x}_0)$；
6. 更新控制输入向量 $\mathbf{x}_0=\mathbf{x}_0+\Delta\mathbf{x}$，回到步骤 2。

MATLAB 代码如下：

function x = newton_raphson()
    % 机器人参数
    m = 1.0;     % 质量
    I = 0.1;     % 转动惯量
    g = 9.8;     % 重力加速度
    f_s = 0.2;   % 静摩擦力系数
    f_k = 0.1;   % 动摩擦力系数
    v_s = 0.1;   % 摩擦力转换速度阈值

    % 收敛阈值
    tol = 1e-6;

    % 初始化控制输入向量
    x0 = [0; m*g; 0];

    while true
        % 计算 F(x0)
        F = [x0(1)-f_s*sign(x0(3));
             x0(2)-m*g;
             x0(3)];

        % 判断是否收敛
        if norm(F) < tol
            x = x0;
            return;
        end

        % 计算 Jacobian 矩阵 J(x0)
        J = [1 0 -f_s*sign(x0(3));
             0 1 0;
             0 0 1];

        % 计算控制输入向量的更新量
        dx = -J\F;

        % 更新控制输入向量
        x0 = x0 + dx;
    end
end

该函数返回的控制输入向量 $\mathbf{x}=[F_x,F_y,M]^T$ 就是机器人在单支撑阶段的不动点。