做多轴机械臂的运动控制，免不了要对4x4的坐标变换矩阵进行乘法，C语言中可以用二维数组或者一维数组来实现矩阵，下面来比较一下二维数组和一维数组的性能差异。
开发环境：Visual Studio 2022，分别在Debug和Release模式下测试函数Multi4x4和Multi16，Release模式下开了最大优化(优选速度)(/O2)，代码速度优先(/Ot)，每种情况重复测3次。(注：最后算出的sum值是2488321446960073699907076096.00)
#include<stdio.h>
#include<stdint.h>
#include<time.h>
void Multi4x4(float A[4][4], float B[4][4], float C[4][4]) {
C[0][0] = A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0];
C[0][1] = A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1];
C[0][2] = A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2];
C[0][3] = A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3];
C[1][0] = A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0];
C[1][1] = A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1];
C[1][2] = A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2];
C[1][3] = A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3];
C[2][0] = A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0];
C[2][1] = A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1];
C[2][2] = A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2];
C[2][3] = A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3];
C[3][0] = A[3][0] * B[0][0] + A[3][1] * B[1][0] + A[3][2] * B[2][0] + A[3][3] * B[3][0];
C[3][1] = A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1];
C[3][2] = A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2];
C[3][3] = A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3];
}
void Multi16(float A[16], float B[16], float C[16]) {
C[0] = A[0] * B[0] + A[1] * B[4] + A[2] * B[8] + A[3] * B[12];
C[1] = A[0] * B[1] + A[1] * B[5] + A[2] * B[9] + A[3] * B[13];
C[2] = A[0] * B[2] + A[1] * B[6] + A[2] * B[10] + A[3] * B[14];
C[3] = A[0] * B[3] + A[1] * B[7] + A[2] * B[11] + A[3] * B[15];
C[4] = A[4] * B[0] + A[5] * B[4] + A[6] * B[8] + A[7] * B[12];
C[5] = A[4] * B[1] + A[5] * B[5] + A[6] * B[9] + A[7] * B[13];
C[6] = A[4] * B[2] + A[5] * B[6] + A[6] * B[10] + A[7] * B[14];
C[7] = A[4] * B[3] + A[5] * B[7] + A[6] * B[11] + A[7] * B[15];
C[8] = A[8] * B[0] + A[9] * B[4] + A[10] * B[8] + A[11] * B[12];
C[9] = A[8] * B[1] + A[9] * B[5] + A[10] * B[9] + A[11] * B[13];
C[10] = A[8] * B[2] + A[9] * B[6] + A[10] * B[10] + A[11] * B[14];
C[11] = A[8] * B[3] + A[9] * B[7] + A[10] * B[11] + A[11] * B[15];
C[12] = A[12] * B[0] + A[13] * B[4] + A[14] * B[8] + A[15] * B[12];
C[13] = A[12] * B[1] + A[13] * B[5] + A[14] * B[9] + A[15] * B[13];
C[14] = A[12] * B[2] + A[13] * B[6] + A[14] * B[10] + A[15] * B[14];
C[15] = A[12] * B[3] + A[13] * B[7] + A[14] * B[11] + A[15] * B[15];
}
int main() {
clock_t start;
double sum = 0.0;
start = clock();
//float a[4][4], b[4][4], c[4][4], d[4][4];
//for (long i = 0; i < 6e7; i++) {
// for (uint8_t j = 0; j < 16; j++) {
// *(a[0] + j) = i * 0.03 + j * 0.02f;
// *(b[0] + j) = i * 0.02 + j * 0.03f;
// }
// Multi4x4(a, b, c);
// Multi4x4(b, c, d);
// sum += d[0][0] + d[1][1] + d[2][2] + d[3][3];
//}
float a[16], b[16], c[16], d[16];
for (long i = 0; i < 6e7; i++) {
for (uint8_t j = 0; j < 16; j++) {
*(a + j) = i * 0.03 + j * 0.02f;
*(b + j) = i * 0.02 + j * 0.03f;
}
Multi16(a, b, c);
Multi16(b, c, d);
sum += d[0] + d[5] + d[10] + d[15];
}
printf("sum=%f, time = %f s\n", sum, (double)(clock() - start) / CLOCKS_PER_SEC);
}耗时对比：
Multi4x4 Multi16
Debug 16.7s 12.1s
Release 2.7s 2.7s
Debug模式下，Multi4x4中C[0][0]=A[0][0]*B[0][0]+A[0][1]*B[1][0]+A[0][2]*B[2][0]+A[0][3]*B[3][0]对应的汇编指令：
mov eax,10h
imul rax,rax,0
mov rcx,qword ptr [A]
add rcx,rax
mov rax,rcx
mov ecx,4
imul rcx,rcx,0
mov edx,10h
imul rdx,rdx,0
mov r8,qword ptr [B]
add r8,rdx
mov rdx,r8
mov r8d,4
imul r8,r8,0
movss xmm0,dword ptr [rax+rcx]
mulss xmm0,dword ptr [rdx+r8]
mov eax,10h
imul rax,rax,0
mov rcx,qword ptr [A]
add rcx,rax
mov rax,rcx
mov ecx,4
imul rcx,rcx,1
mov edx,10h
imul rdx,rdx,1
mov r8,qword ptr [B]
add r8,rdx
mov rdx,r8
mov r8d,4
imul r8,r8,0
movss xmm1,dword ptr [rax+rcx]
mulss xmm1,dword ptr [rdx+r8]
addss xmm0,xmm1
mov eax,10h
imul rax,rax,0
mov rcx,qword ptr [A]
add rcx,rax
mov rax,rcx
mov ecx,4
imul rcx,rcx,2
mov edx,10h
imul rdx,rdx,2
mov r8,qword ptr [B]
add r8,rdx
mov rdx,r8
mov r8d,4
imul r8,r8,0
movss xmm1,dword ptr [rax+rcx]
mulss xmm1,dword ptr [rdx+r8]
addss xmm0,xmm1
mov eax,10h
imul rax,rax,0
mov rcx,qword ptr [A]
add rcx,rax
mov rax,rcx
mov ecx,4
imul rcx,rcx,3
mov edx,10h
imul rdx,rdx,3
mov r8,qword ptr [B]
add r8,rdx
mov rdx,r8
mov r8d,4
imul r8,r8,0
movss xmm1,dword ptr [rax+rcx]
mulss xmm1,dword ptr [rdx+r8]
addss xmm0,xmm1
mov eax,10h
imul rax,rax,0
mov rcx,qword ptr [C]
add rcx,rax
mov rax,rcx
mov ecx,4
imul rcx,rcx,0
movss dword ptr [rax+rcx],xmm0Debug模式下，Multi16中C[0]=A[0]*B[0]+A[1]*B[4]+A[2]*B[8]+A[3]*B[12]对应的汇编指令：
mov eax,4
imul rax,rax,0
mov ecx,4
imul rcx,rcx,0
mov rdx,qword ptr [A]
mov r8,qword ptr [B]
movss xmm0,dword ptr [rdx+rax]
mulss xmm0,dword ptr [r8+rcx]
mov eax,4
imul rax,rax,1
mov ecx,4
imul rcx,rcx,4
mov rdx,qword ptr [A]
mov r8,qword ptr [B]
movss xmm1,dword ptr [rdx+rax]
mulss xmm1,dword ptr [r8+rcx]
addss xmm0,xmm1
mov eax,4
imul rax,rax,2
mov ecx,4
imul rcx,rcx,8
mov rdx,qword ptr [A]
mov r8,qword ptr [B]
movss xmm1,dword ptr [rdx+rax]
mulss xmm1,dword ptr [r8+rcx]
addss xmm0,xmm1
mov eax,4
imul rax,rax,3
mov ecx,4
imul rcx,rcx,0Ch
mov rdx,qword ptr [A]
mov r8,qword ptr [B]
movss xmm1,dword ptr [rdx+rax]
mulss xmm1,dword ptr [r8+rcx]
addss xmm0,xmm1
mov eax,4
imul rax,rax,0
mov rcx,qword ptr [C]
movss dword ptr [rcx+rax],xmm0Release模式下，Multi4x4和Multi16的汇编指令一样：
movss xmm5,dword ptr [rcx+4]
movss xmm4,dword ptr [rcx+8]
movss xmm3,dword ptr [rcx]
movaps xmm1,xmm4
mulss xmm1,dword ptr [rdx+20h]
movss xmm2,dword ptr [rdx]
movss xmm0,dword ptr [rdx+10h]
mulss xmm2,xmm3
mulss xmm0,xmm5
movaps xmmword ptr [rax-18h],xmm6
movss xmm6,dword ptr [rcx+0Ch]
addss xmm2,xmm0
movaps xmmword ptr [rax-28h],xmm7结论：在Debug模式下，一维数组的汇编指令(39行)比二维数组(75行)几乎少一半，速度更快。因为访问二维数组的元素需要先找到其所在的行，再找到其所在的列，相当于进行了两次数组访问。在Release模式下，两者的汇编指令一样，都用了SSE指令集进行优化加速，速度一样。但SSE指令是intel和AMD等电脑CPU才有的(Arm的Neon类似于SSE)，单片机可没法用。因此，如果想在单片机上获得极致的速度，那还是使用一维数组吧。文章来源：https://www.toymoban.com/news/detail-843131.html
按照D-H表示法建立的4x4矩阵，第4行始终为[0, 0, 0, 1]，做乘法以后也不会改变，因此，专门做此类4x4矩阵乘法的函数可以进一步优化，即只取前12个元素参与运算。文章来源地址https://www.toymoban.com/news/detail-843131.html
void Multi16(float A[12], float B[12], float C[12]) {
C[0] = A[0] * B[0] + A[1] * B[4] + A[2] * B[8] ;
C[1] = A[0] * B[1] + A[1] * B[5] + A[2] * B[9] ;
C[2] = A[0] * B[2] + A[1] * B[6] + A[2] * B[10] ;
C[3] = A[0] * B[3] + A[1] * B[7] + A[2] * B[11] + A[3] ;
C[4] = A[4] * B[0] + A[5] * B[4] + A[6] * B[8] ;
C[5] = A[4] * B[1] + A[5] * B[5] + A[6] * B[9] ;
C[6] = A[4] * B[2] + A[5] * B[6] + A[6] * B[10] ;
C[7] = A[4] * B[3] + A[5] * B[7] + A[6] * B[11] + A[7] ;
C[8] = A[8] * B[0] + A[9] * B[4] + A[10] * B[8] ;
C[9] = A[8] * B[1] + A[9] * B[5] + A[10] * B[9] ;
C[10] = A[8] * B[2] + A[9] * B[6] + A[10] * B[10] ;
C[11] = A[8] * B[3] + A[9] * B[7] + A[10] * B[11] + A[11] ;
}到了这里，关于多轴机械臂运动控制：4x4坐标变换矩阵该用C语言的二维数组还是一维数组？的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
