递推的Newton-Euler动力学方法matlab、C语言网站首页 学无止境

递推的Newton-Euler动力学方法matlab、C语言

Martlet119 2025-03-31 00:01:03

简介递推的Newton-Euler动力学方法matlab、C语言

function [tau] = newton_euler_dynamics(q, qd, qdd, m, r, I, g)
    % q: 关节角度 (n x 1)
    % qd: 关节角速度 (n x 1)
    % qdd: 关节角加速度 (n x 1)
    % m: 连杆质量 (n x 1)
    % r: 连杆质心位置 (n x 3)
    % I: 连杆惯性矩阵 (n x 3 x 3)
    % g: 重力加速度 (3 x 1)
    
    n = length(q); % 关节数量
    w = zeros(3, n); % 角速度
    wd = zeros(3, n); % 角加速度
    v = zeros(3, n); % 线速度
    vd = zeros(3, n); % 线加速度
    f = zeros(3, n); % 力
    tau = zeros(n, 1); % 关节力矩
    
    % 前向递推
    for i = 1:n
        if i == 1
            w(:, i) = [0; 0; qd(i)];
            wd(:, i) = [0; 0; qdd(i)];
            vd(:, i) = [0; 0; 0] + cross(wd(:, i), r(i, :)') + cross(w(:, i), cross(w(:, i), r(i, :)')) + g;
        else
            w(:, i) = w(:, i-1) + [0; 0; qd(i)];
            wd(:, i) = wd(:, i-1) + cross(w(:, i-1), [0; 0; qd(i)]) + [0; 0; qdd(i)];
            vd(:, i) = vd(:, i-1) + cross(wd(:, i), r(i, :)') + cross(w(:, i), cross(w(:, i), r(i, :)'));
        end
    end
    
    % 反向递推
    for i = n:-1:1
        f(:, i) = m(i) * vd(:, i);
        if i < n
            f(:, i) = f(:, i) + f(:, i+1);
        end
        tau(i) = dot(f(:, i), r(i, :)') + dot(wd(:, i), I(i, :, :) * w(:, i)) + cross(w(:, i), I(i, :, :) * w(:, i));
    end
end

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define N 3 // 关节数量

void newton_euler_dynamics(double q[], double qd[], double qdd[], double m[], double r[][3], double I[][3][3], double g[], double tau[]) {
    double w[3][N] = {0}; // 角速度
    double wd[3][N] = {0}; // 角加速度
    double vd[3][N] = {0}; // 线加速度
    double f[3][N] = {0}; // 力
    double cross_temp[3] = {0}; // 临时存储叉积结果

    // 前向递推
    for (int i = 0; i < N; i++) {
        if (i == 0) {
            w[2][i] = qd[i];
            wd[2][i] = qdd[i];
            vd[0][i] = g[0];
            vd[1][i] = g[1];
            vd[2][i] = g[2];
        } else {
            w[0][i] = w[0][i-1];
            w[1][i] = w[1][i-1];
            w[2][i] = w[2][i-1] + qd[i];

            wd[0][i] = wd[0][i-1];
            wd[1][i] = wd[1][i-1];
            wd[2][i] = wd[2][i-1] + qdd[i];

            // 计算线加速度
            cross_temp[0] = wd[1][i] * r[i][2] - wd[2][i] * r[i][1];
            cross_temp[1] = wd[2][i] * r[i][0] - wd[0][i] * r[i][2];
            cross_temp[2] = wd[0][i] * r[i][1] - wd[1][i] * r[i][0];

            vd[0][i] = vd[0][i-1] + cross_temp[0];
            vd[1][i] = vd[1][i-1] + cross_temp[1];
            vd[2][i] = vd[2][i-1] + cross_temp[2];
        }
    }

    // 反向递推
    for (int i = N-1; i >= 0; i--) {
        f[0][i] = m[i] * vd[0][i];
        f[1][i] = m[i] * vd[1][i];
        f[2][i] = m[i] * vd[2][i];

        if (i < N-1) {
            f[0][i] += f[0][i+1];
            f[1][i] += f[1][i+1];
            f[2][i] += f[2][i+1];
        }

        // 计算关节力矩
        tau[i] = f[0][i] * r[i][0] + f[1][i] * r[i][1] + f[2][i] * r[i][2];
        tau[i] += wd[0][i] * I[i][0][0] * w[0][i] + wd[1][i] * I[i][1][1] * w[1][i] + wd[2][i] * I[i][2][2] * w[2][i];
    }
}

int main() {
    double q[N] = {0.1, 0.2, 0.3}; // 关节角度
    double qd[N] = {0.1, 0.2, 0.3}; // 关节角速度
    double qdd[N] = {0.1, 0.2, 0.3}; // 关节角加速度
    double m[N] = {1.0, 1.0, 1.0}; // 连杆质量
    double r[N][3] = {
  
  {0.1, 0.2, 0.3}, {0.1, 0.2, 0.3}, {0.1, 0.2, 0.3}}; // 连杆质心位置
    double I[N][3][3] = {
        {
  
  {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}},
        {
  
  {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}},
        {
  
  {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}
    }; // 连杆惯性矩阵
    double g[3] = {0.0, 0.0, -9.81}; // 重力加速度
    double tau[N] = {0}; // 关节力矩

    newton_euler_dynamics(q, qd, qdd, m, r, I, g, tau);

    for (int i = 0; i < N; i++) {
        printf("tau[%d] = %f
", i, tau[i]);
    }

    return 0;
}

代码说明

MATLAB 代码：
- newton_euler_dynamics 函数实现了递推牛顿-欧拉动力学算法。
- 前向递推计算每个连杆的角速度和线加速度。
- 反向递推计算每个关节的力和力矩。
C 语言代码：
- newton_euler_dynamics 函数实现了递推牛顿-欧拉动力学算法。
- 前向递推计算每个连杆的角速度和线加速度。
- 反向递推计算每个关节的力和力矩。
- main 函数中初始化了一些示例数据，并调用 newton_euler_dynamics 函数计算关节力矩。